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.
This commit is contained in:
parent
9a8a31676b
commit
889b14df5f
|
@ -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) {
|
||||
|
|
Loading…
Reference in New Issue