From ea3184c2657b472f0fdb6635a0a690f3266eafdc Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Fri, 23 Feb 2018 12:13:17 -0500 Subject: [PATCH] Fix 3D on a switch throttle inversion/scale There was a logic inversion when 3d_switched_mode = ON causeing the motor directions to be inverted. Also the trottle scaling for reverse thrust was being applied twice from both the 3d_switched_mode = ON and 3d_switched_mode = OFF sections. --- src/main/fc/fc_rc.c | 28 +++++++++++++++------------- 1 file changed, 15 insertions(+), 13 deletions(-) diff --git a/src/main/fc/fc_rc.c b/src/main/fc/fc_rc.c index c97bac0e86..e396c39f65 100644 --- a/src/main/fc/fc_rc.c +++ b/src/main/fc/fc_rc.c @@ -324,20 +324,22 @@ void updateRcCommands(void) rcCommand[THROTTLE] = rcLookupThrottle(tmp); - if (feature(FEATURE_3D) && IS_RC_MODE_ACTIVE(BOX3D) && !failsafeIsActive()) { - fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); - rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc); - } - - if (feature(FEATURE_3D) && flight3DConfig()->switched_mode3d && !failsafeIsActive()) { - if (IS_RC_MODE_ACTIVE(BOX3D)) { - reverseMotors = false; - fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); - rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc); + if (feature(FEATURE_3D) && !failsafeIsActive()) { + if (!flight3DConfig()->switched_mode3d) { + if (IS_RC_MODE_ACTIVE(BOX3D)) { + fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); + rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc); + } } else { - reverseMotors = true; - fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); - rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MIN - rxConfig()->midrc); + if (IS_RC_MODE_ACTIVE(BOX3D)) { + reverseMotors = true; + fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); + rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MIN - rxConfig()->midrc); + } else { + reverseMotors = false; + fix12_t throttleScaler = qConstruct(rcCommand[THROTTLE] - 1000, 1000); + rcCommand[THROTTLE] = rxConfig()->midrc + qMultiply(throttleScaler, PWM_RANGE_MAX - rxConfig()->midrc); + } } } if (FLIGHT_MODE(HEADFREE_MODE)) {