Commit 889b14df authored by Dominic Clifton's avatar Dominic Clifton
Browse files

Support pan and tilt on fixed wing. Fixes #1006

The general idea is that nothing calls pwmWriteServo except for
writeServos() and that writeServos() knows which servos it has used.
parent 9a8a3167
......@@ -431,21 +431,14 @@ void mixerResetMotors(void)
#ifdef USE_SERVOS
STATIC_UNIT_TESTED void forwardAuxChannelsToServos(void)
STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)
{
// offset servos based off number already used in mixer types
// airplane and servo_tilt together can't be used
int8_t firstServo = servoCount - AUX_FORWARD_CHANNEL_TO_SERVO_COUNT;
// start forwarding from this channel
uint8_t channelOffset = AUX1;
int8_t servoOffset;
uint8_t servoOffset;
for (servoOffset = 0; servoOffset < AUX_FORWARD_CHANNEL_TO_SERVO_COUNT; servoOffset++) {
if (firstServo + servoOffset < 0) {
continue; // there are not enough servos to forward all the AUX channels.
}
pwmWriteServo(firstServo + servoOffset, rcData[channelOffset++]);
pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]);
}
}
......@@ -460,48 +453,52 @@ void writeServos(void)
if (!useServo)
return;
uint8_t servoIndex = 0;
switch (currentMixerMode) {
case MIXER_BI:
pwmWriteServo(0, servo[SERVO_BIPLANE_LEFT]);
pwmWriteServo(1, servo[SERVO_BIPLANE_RIGHT]);
pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_LEFT]);
pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_RIGHT]);
break;
case MIXER_TRI:
if (mixerConfig->tri_unarmed_servo) {
// if unarmed flag set, we always move servo
pwmWriteServo(0, servo[SERVO_RUDDER]);
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
} else {
// otherwise, only move servo when copter is armed
if (ARMING_FLAG(ARMED))
pwmWriteServo(0, servo[SERVO_RUDDER]);
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
else
pwmWriteServo(0, 0); // kill servo signal completely.
pwmWriteServo(servoIndex++, 0); // kill servo signal completely.
}
break;
case MIXER_FLYING_WING:
pwmWriteServo(0, servo[SERVO_FLAPPERON_1]);
pwmWriteServo(1, servo[SERVO_FLAPPERON_2]);
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]);
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]);
break;
case MIXER_GIMBAL:
updateGimbalServos();
servoIndex += 2;
break;
case MIXER_DUALCOPTER:
pwmWriteServo(0, servo[SERVO_DUALCOPTER_LEFT]);
pwmWriteServo(1, servo[SERVO_DUALCOPTER_RIGHT]);
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]);
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
break;
case MIXER_AIRPLANE:
for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
pwmWriteServo(i - SERVO_PLANE_INDEX_MIN, servo[i]);
pwmWriteServo(servoIndex++, servo[i]);
}
break;
case MIXER_SINGLECOPTER:
for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) {
pwmWriteServo(i - SERVO_SINGLECOPTER_INDEX_MIN, servo[i]);
pwmWriteServo(servoIndex++, servo[i]);
}
break;
......@@ -509,9 +506,16 @@ void writeServos(void)
// Two servos for SERVO_TILT, if enabled
if (feature(FEATURE_SERVO_TILT)) {
updateGimbalServos();
servoIndex += 2;
}
break;
}
// forward AUX1-4 to servo outputs (not constrained)
if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) {
forwardAuxChannelsToServos(servoIndex);
servoIndex += AUX_FORWARD_CHANNEL_TO_SERVO_COUNT;
}
}
#endif
......@@ -710,10 +714,6 @@ void mixTable(void)
servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
}
}
// forward AUX1-4 to servo outputs (not constrained)
if (gimbalConfig->gimbal_flags & GIMBAL_FORWARDAUX) {
forwardAuxChannelsToServos();
}
#endif
if (ARMING_FLAG(ARMED)) {
......
......@@ -46,7 +46,7 @@ extern "C" {
#include "io/rc_controls.h"
extern uint8_t servoCount;
void forwardAuxChannelsToServos(void);
void forwardAuxChannelsToServos(uint8_t firstServoIndex);
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers);
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
......@@ -90,7 +90,7 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithNoServos)
rcData[AUX4] = TEST_RC_MID;
// when
forwardAuxChannelsToServos();
forwardAuxChannelsToServos(MAX_SUPPORTED_SERVOS);
// then
for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
......@@ -110,26 +110,20 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithMaxServos)
rcData[AUX4] = 2000;
// when
forwardAuxChannelsToServos();
forwardAuxChannelsToServos(MAX_SUPPORTED_SERVOS);
// then
uint8_t i;
for (i = 0; i < MAX_SUPPORTED_SERVOS - 4; i++) {
EXPECT_EQ(servos[i].value, 0);
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
EXPECT_EQ(0, servos[i].value);
}
// -1 for zero based offset
EXPECT_EQ(1000, servos[MAX_SUPPORTED_SERVOS - 3 - 1].value);
EXPECT_EQ(1250, servos[MAX_SUPPORTED_SERVOS - 2 - 1].value);
EXPECT_EQ(1750, servos[MAX_SUPPORTED_SERVOS - 1 - 1].value);
EXPECT_EQ(2000, servos[MAX_SUPPORTED_SERVOS - 0 - 1].value);
}
TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithLessServosThanAuxChannelsToForward)
TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithLessRemainingServosThanAuxChannelsToForward)
{
// given
memset(&servos, 0, sizeof(servos));
servoCount = 2;
servoCount = MAX_SUPPORTED_SERVOS - 2;
rcData[AUX1] = 1000;
rcData[AUX2] = 1250;
......@@ -137,17 +131,17 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithLessServosThanAuxChannel
rcData[AUX4] = 2000;
// when
forwardAuxChannelsToServos();
forwardAuxChannelsToServos(MAX_SUPPORTED_SERVOS - 2);
// then
uint8_t i;
for (i = 2; i < MAX_SUPPORTED_SERVOS; i++) {
EXPECT_EQ(servos[i].value, 0);
for (i = 0; i < MAX_SUPPORTED_SERVOS - 2; i++) {
EXPECT_EQ(0, servos[i].value);
}
// -1 for zero based offset
EXPECT_EQ(1000, servos[0].value);
EXPECT_EQ(1250, servos[1].value);
EXPECT_EQ(1000, servos[MAX_SUPPORTED_SERVOS - 1 - 1].value);
EXPECT_EQ(1250, servos[MAX_SUPPORTED_SERVOS - 0 - 1].value);
}
TEST(FlightMixerTest, TestTricopterServo)
......@@ -317,7 +311,13 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) {
}
void pwmWriteServo(uint8_t index, uint16_t value) {
servos[index].value = value;
// FIXME logic in test, mimic's production code.
// Perhaps the solution is to remove the logic from the production code version and assume that
// anything calling calling pwmWriteServo always uses a valid index?
// See MAX_SERVOS in pwm_output (driver) and MAX_SUPPORTED_SERVOS (flight)
if (index < MAX_SERVOS) {
servos[index].value = value;
}
}
bool failsafeIsActive(void) {
......
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment