mirror of
https://github.com/betaflight/betaflight.git
synced 2025-07-23 00:05:33 +03:00
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
2 changed files with 44 additions and 44 deletions
|
@ -431,21 +431,14 @@ void mixerResetMotors(void)
|
||||||
|
|
||||||
#ifdef USE_SERVOS
|
#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
|
// start forwarding from this channel
|
||||||
uint8_t channelOffset = AUX1;
|
uint8_t channelOffset = AUX1;
|
||||||
|
|
||||||
int8_t servoOffset;
|
uint8_t servoOffset;
|
||||||
for (servoOffset = 0; servoOffset < AUX_FORWARD_CHANNEL_TO_SERVO_COUNT; servoOffset++) {
|
for (servoOffset = 0; servoOffset < AUX_FORWARD_CHANNEL_TO_SERVO_COUNT; servoOffset++) {
|
||||||
if (firstServo + servoOffset < 0) {
|
pwmWriteServo(firstServoIndex + servoOffset, rcData[channelOffset++]);
|
||||||
continue; // there are not enough servos to forward all the AUX channels.
|
|
||||||
}
|
|
||||||
pwmWriteServo(firstServo + servoOffset, rcData[channelOffset++]);
|
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -460,48 +453,52 @@ void writeServos(void)
|
||||||
if (!useServo)
|
if (!useServo)
|
||||||
return;
|
return;
|
||||||
|
|
||||||
|
uint8_t servoIndex = 0;
|
||||||
|
|
||||||
switch (currentMixerMode) {
|
switch (currentMixerMode) {
|
||||||
case MIXER_BI:
|
case MIXER_BI:
|
||||||
pwmWriteServo(0, servo[SERVO_BIPLANE_LEFT]);
|
pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_LEFT]);
|
||||||
pwmWriteServo(1, servo[SERVO_BIPLANE_RIGHT]);
|
pwmWriteServo(servoIndex++, servo[SERVO_BIPLANE_RIGHT]);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MIXER_TRI:
|
case MIXER_TRI:
|
||||||
if (mixerConfig->tri_unarmed_servo) {
|
if (mixerConfig->tri_unarmed_servo) {
|
||||||
// if unarmed flag set, we always move servo
|
// if unarmed flag set, we always move servo
|
||||||
pwmWriteServo(0, servo[SERVO_RUDDER]);
|
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
|
||||||
} else {
|
} else {
|
||||||
// otherwise, only move servo when copter is armed
|
// otherwise, only move servo when copter is armed
|
||||||
if (ARMING_FLAG(ARMED))
|
if (ARMING_FLAG(ARMED))
|
||||||
pwmWriteServo(0, servo[SERVO_RUDDER]);
|
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
|
||||||
else
|
else
|
||||||
pwmWriteServo(0, 0); // kill servo signal completely.
|
pwmWriteServo(servoIndex++, 0); // kill servo signal completely.
|
||||||
}
|
}
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MIXER_FLYING_WING:
|
case MIXER_FLYING_WING:
|
||||||
pwmWriteServo(0, servo[SERVO_FLAPPERON_1]);
|
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]);
|
||||||
pwmWriteServo(1, servo[SERVO_FLAPPERON_2]);
|
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MIXER_GIMBAL:
|
case MIXER_GIMBAL:
|
||||||
updateGimbalServos();
|
updateGimbalServos();
|
||||||
|
servoIndex += 2;
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MIXER_DUALCOPTER:
|
case MIXER_DUALCOPTER:
|
||||||
pwmWriteServo(0, servo[SERVO_DUALCOPTER_LEFT]);
|
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]);
|
||||||
pwmWriteServo(1, servo[SERVO_DUALCOPTER_RIGHT]);
|
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
|
||||||
break;
|
break;
|
||||||
|
|
||||||
case MIXER_AIRPLANE:
|
case MIXER_AIRPLANE:
|
||||||
for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
|
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;
|
break;
|
||||||
|
|
||||||
case MIXER_SINGLECOPTER:
|
case MIXER_SINGLECOPTER:
|
||||||
for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) {
|
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;
|
break;
|
||||||
|
|
||||||
|
@ -509,9 +506,16 @@ void writeServos(void)
|
||||||
// Two servos for SERVO_TILT, if enabled
|
// Two servos for SERVO_TILT, if enabled
|
||||||
if (feature(FEATURE_SERVO_TILT)) {
|
if (feature(FEATURE_SERVO_TILT)) {
|
||||||
updateGimbalServos();
|
updateGimbalServos();
|
||||||
|
servoIndex += 2;
|
||||||
}
|
}
|
||||||
break;
|
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
|
#endif
|
||||||
|
|
||||||
|
@ -710,10 +714,6 @@ void mixTable(void)
|
||||||
servo[i] = constrain(servo[i], servoConf[i].min, servoConf[i].max); // limit the values
|
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
|
#endif
|
||||||
|
|
||||||
if (ARMING_FLAG(ARMED)) {
|
if (ARMING_FLAG(ARMED)) {
|
||||||
|
|
|
@ -46,7 +46,7 @@ extern "C" {
|
||||||
#include "io/rc_controls.h"
|
#include "io/rc_controls.h"
|
||||||
|
|
||||||
extern uint8_t servoCount;
|
extern uint8_t servoCount;
|
||||||
void forwardAuxChannelsToServos(void);
|
void forwardAuxChannelsToServos(uint8_t firstServoIndex);
|
||||||
|
|
||||||
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers);
|
void mixerInit(mixerMode_e mixerMode, motorMixer_t *initialCustomMixers);
|
||||||
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
|
void mixerUsePWMOutputConfiguration(pwmOutputConfiguration_t *pwmOutputConfiguration);
|
||||||
|
@ -90,7 +90,7 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithNoServos)
|
||||||
rcData[AUX4] = TEST_RC_MID;
|
rcData[AUX4] = TEST_RC_MID;
|
||||||
|
|
||||||
// when
|
// when
|
||||||
forwardAuxChannelsToServos();
|
forwardAuxChannelsToServos(MAX_SUPPORTED_SERVOS);
|
||||||
|
|
||||||
// then
|
// then
|
||||||
for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
for (uint8_t i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||||
|
@ -110,26 +110,20 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithMaxServos)
|
||||||
rcData[AUX4] = 2000;
|
rcData[AUX4] = 2000;
|
||||||
|
|
||||||
// when
|
// when
|
||||||
forwardAuxChannelsToServos();
|
forwardAuxChannelsToServos(MAX_SUPPORTED_SERVOS);
|
||||||
|
|
||||||
// then
|
// then
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
for (i = 0; i < MAX_SUPPORTED_SERVOS - 4; i++) {
|
for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
|
||||||
EXPECT_EQ(servos[i].value, 0);
|
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
|
// given
|
||||||
memset(&servos, 0, sizeof(servos));
|
memset(&servos, 0, sizeof(servos));
|
||||||
servoCount = 2;
|
servoCount = MAX_SUPPORTED_SERVOS - 2;
|
||||||
|
|
||||||
rcData[AUX1] = 1000;
|
rcData[AUX1] = 1000;
|
||||||
rcData[AUX2] = 1250;
|
rcData[AUX2] = 1250;
|
||||||
|
@ -137,17 +131,17 @@ TEST(FlightMixerTest, TestForwardAuxChannelsToServosWithLessServosThanAuxChannel
|
||||||
rcData[AUX4] = 2000;
|
rcData[AUX4] = 2000;
|
||||||
|
|
||||||
// when
|
// when
|
||||||
forwardAuxChannelsToServos();
|
forwardAuxChannelsToServos(MAX_SUPPORTED_SERVOS - 2);
|
||||||
|
|
||||||
// then
|
// then
|
||||||
uint8_t i;
|
uint8_t i;
|
||||||
for (i = 2; i < MAX_SUPPORTED_SERVOS; i++) {
|
for (i = 0; i < MAX_SUPPORTED_SERVOS - 2; i++) {
|
||||||
EXPECT_EQ(servos[i].value, 0);
|
EXPECT_EQ(0, servos[i].value);
|
||||||
}
|
}
|
||||||
|
|
||||||
// -1 for zero based offset
|
// -1 for zero based offset
|
||||||
EXPECT_EQ(1000, servos[0].value);
|
EXPECT_EQ(1000, servos[MAX_SUPPORTED_SERVOS - 1 - 1].value);
|
||||||
EXPECT_EQ(1250, servos[1].value);
|
EXPECT_EQ(1250, servos[MAX_SUPPORTED_SERVOS - 0 - 1].value);
|
||||||
}
|
}
|
||||||
|
|
||||||
TEST(FlightMixerTest, TestTricopterServo)
|
TEST(FlightMixerTest, TestTricopterServo)
|
||||||
|
@ -317,7 +311,13 @@ void pwmCompleteOneshotMotorUpdate(uint8_t motorCount) {
|
||||||
}
|
}
|
||||||
|
|
||||||
void pwmWriteServo(uint8_t index, uint16_t value) {
|
void pwmWriteServo(uint8_t index, uint16_t 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;
|
servos[index].value = value;
|
||||||
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
bool failsafeIsActive(void) {
|
bool failsafeIsActive(void) {
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue