Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-24 16:55:36 +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)
{
pwmWriteServo(firstServoIndex + 0, servo[SERVO_GIMBAL_PITCH]);
pwmWriteServo(firstServoIndex + 1, servo[SERVO_GIMBAL_ROLL]);
writeServoWithTracking(firstServoIndex + 0, SERVO_GIMBAL_PITCH);
writeServoWithTracking(firstServoIndex + 1, SERVO_GIMBAL_ROLL);
}
static void servoTable(void);
@ -324,51 +336,46 @@ void writeServos(void)
switch (currentMixerMode) {
case MIXER_TRI:
case MIXER_CUSTOM_TRI:
if (servosTricopterIsEnabledServoUnarmed()) {
// if unarmed flag set, we always move servo
pwmWriteServo(servoIndex++, servo[SERVO_RUDDER]);
} 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.
// We move servo if unarmed flag set or armed
if (!(servosTricopterIsEnabledServoUnarmed() || ARMING_FLAG(ARMED))) {
servo[SERVO_RUDDER] = 0; // kill servo signal completely.
}
writeServoWithTracking(servoIndex++, SERVO_RUDDER);
break;
case MIXER_FLYING_WING:
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_1]);
pwmWriteServo(servoIndex++, servo[SERVO_FLAPPERON_2]);
writeServoWithTracking(servoIndex++, SERVO_FLAPPERON_1);
writeServoWithTracking(servoIndex++, SERVO_FLAPPERON_2);
break;
case MIXER_CUSTOM_AIRPLANE:
case MIXER_AIRPLANE:
for (int i = SERVO_PLANE_INDEX_MIN; i <= SERVO_PLANE_INDEX_MAX; i++) {
pwmWriteServo(servoIndex++, servo[i]);
writeServoWithTracking(servoIndex++, i);
}
break;
#ifdef USE_UNCOMMON_MIXERS
case MIXER_BICOPTER:
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_LEFT]);
pwmWriteServo(servoIndex++, servo[SERVO_BICOPTER_RIGHT]);
writeServoWithTracking(servoIndex++, SERVO_BICOPTER_LEFT);
writeServoWithTracking(servoIndex++, SERVO_BICOPTER_RIGHT);
break;
case MIXER_HELI_120_CCPM:
pwmWriteServo(servoIndex++, servo[SERVO_HELI_LEFT]);
pwmWriteServo(servoIndex++, servo[SERVO_HELI_RIGHT]);
pwmWriteServo(servoIndex++, servo[SERVO_HELI_TOP]);
pwmWriteServo(servoIndex++, servo[SERVO_HELI_RUD]);
writeServoWithTracking(servoIndex++, SERVO_HELI_LEFT);
writeServoWithTracking(servoIndex++, SERVO_HELI_RIGHT);
writeServoWithTracking(servoIndex++, SERVO_HELI_TOP);
writeServoWithTracking(servoIndex++, SERVO_HELI_RUD);
break;
case MIXER_DUALCOPTER:
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_LEFT]);
pwmWriteServo(servoIndex++, servo[SERVO_DUALCOPTER_RIGHT]);
writeServoWithTracking(servoIndex++, SERVO_DUALCOPTER_LEFT);
writeServoWithTracking(servoIndex++, SERVO_DUALCOPTER_RIGHT);
break;
case MIXER_SINGLECOPTER:
for (int i = SERVO_SINGLECOPTER_INDEX_MIN; i <= SERVO_SINGLECOPTER_INDEX_MAX; i++) {
pwmWriteServo(servoIndex++, servo[i]);
writeServoWithTracking(servoIndex++, i);
}
break;
#endif // USE_UNCOMMON_MIXERS
@ -383,6 +390,14 @@ void writeServos(void)
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)
if (featureIsEnabled(FEATURE_CHANNEL_FORWARDING)) {
forwardAuxChannelsToServos(servoIndex);