Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-25 17:25:20 +03:00
Code Issues Wiki Activity

Merge pull request #8792 from jflyper/bfdev-fix-servo-to-write-...

Browse source 
Write forward only servos
This commit is contained in:
Michael Keller 2019-09-03 23:59:24 +12:00 • committed by GitHub
commit 37b059532f
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
1 changed files with 38 additions and 23 deletions
Download patch file Download diff file Expand all files Collapse all files

61
src/main/flight/servos.c
View file

@ -306,10 +306,22 @@ STATIC_UNIT_TESTED void forwardAuxChannelsToServos(uint8_t firstServoIndex)
} }
} }
// Write and keep track of written servos
static uint32_t servoWritten;
STATIC_ASSERT(sizeof(servoWritten) * 8 >= MAX_SUPPORTED_SERVOS, servoWritten_is_too_small);
static void writeServoWithTracking(uint8_t index, servoIndex_e servoname)
{
pwmWriteServo(index, servo[servoname]);
servoWritten |= (1 << servoname);
}
static void updateGimbalServos(uint8_t firstServoIndex) static void updateGimbalServos(uint8_t firstServoIndex)
{ {
pwmWriteServo(firstServoIndex + 0, servo[SERVO_GIMBAL_PITCH]); writeServoWithTracking(firstServoIndex + 0, SERVO_GIMBAL_PITCH);
pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]); writeServoWithTracking(firstServoIndex + 1, SERVO_GIMBAL_ROLL);
} }
static void servoTable(void); static void servoTable(void);
@ -324,51 +336,46 @@ void writeServos(void)
switch (currentMixerMode) { switch (currentMixerMode) {
case MIXER_TRI: case MIXER_TRI:
case MIXER_CUSTOM_TRI: case MIXER_CUSTOM_TRI:
if (servosTricopterIsEnabledServoUnarmed()) { // We move servo if unarmed flag set or armed
// if unarmed flag set, we always move servo if (!(servosTricopterIsEnabledServoUnarmed() || ARMING_FLAG(ARMED))) {
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]); servo[SERVO_RUDDER] = 0; // kill servo signal completely.
} else {
// otherwise, only move servo when copter is armed
if (ARMING_FLAG(ARMED))
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
else
pwmWriteServo(servoIndex++, 0); // kill servo signal completely.
} }
writeServoWithTracking(servoIndex++, SERVO_RUDDER);
break; break;
case MIXER_FLYING_WING: case MIXER_FLYING_WING:
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]); writeServoWithTracking(servoIndex++, SERVO_FLAPPERON_1);
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]); writeServoWithTracking(servoIndex++, SERVO_FLAPPERON_2);
break; break;
case MIXER_CUSTOM_AIRPLANE: case MIXER_CUSTOM_AIRPLANE:
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(servoIndex++, servo[i]); writeServoWithTracking(servoIndex++, i);
} }
break; break;
#ifdef USE_UNCOMMON_MIXERS #ifdef USE_UNCOMMON_MIXERS
case MIXER_BICOPTER: case MIXER_BICOPTER:
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]); writeServoWithTracking(servoIndex++, SERVO_BICOPTER_LEFT);
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]); writeServoWithTracking(servoIndex++, SERVO_BICOPTER_RIGHT);
break; break;
case MIXER_HELI_120_CCPM: case MIXER_HELI_120_CCPM:
pwmWriteServo(servoIndex++, servo[SERVO_HELI_LEFT]); writeServoWithTracking(servoIndex++, SERVO_HELI_LEFT);
pwmWriteServo(servoIndex++, servo[SERVO_HELI_RIGHT]); writeServoWithTracking(servoIndex++, SERVO_HELI_RIGHT);
pwmWriteServo(servoIndex++, servo[SERVO_HELI_TOP]); writeServoWithTracking(servoIndex++, SERVO_HELI_TOP);
pwmWriteServo(servoIndex++, servo[SERVO_HELI_RUD]); writeServoWithTracking(servoIndex++, SERVO_HELI_RUD);
break; break;
case MIXER_DUALCOPTER: case MIXER_DUALCOPTER:
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]); writeServoWithTracking(servoIndex++, SERVO_DUALCOPTER_LEFT);
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]); writeServoWithTracking(servoIndex++, SERVO_DUALCOPTER_RIGHT);
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(servoIndex++, servo[i]); writeServoWithTracking(servoIndex++, i);
} }
break; break;
#endif // USE_UNCOMMON_MIXERS #endif // USE_UNCOMMON_MIXERS
@ -383,6 +390,14 @@ void writeServos(void)
servoIndex += 2; servoIndex += 2;
} }
// Scan servos and write those marked forwarded and not written yet
for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) {
const uint8_t channelToForwardFrom = servoParams(i)->forwardFromChannel;
if ((channelToForwardFrom != CHANNEL_FORWARDING_DISABLED) && !(servoWritten & (1 << i))) {
pwmWriteServo(servoIndex++, servo[i]);
}
}
// forward AUX to remaining servo outputs (not constrained) // forward AUX to remaining servo outputs (not constrained)
if (featureIsEnabled(FEATURE_CHANNEL_FORWARDING)) { if (featureIsEnabled(FEATURE_CHANNEL_FORWARDING)) {
forwardAuxChannelsToServos(servoIndex); forwardAuxChannelsToServos(servoIndex);