diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 02236a25b6..7fd4ea661c 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -570,28 +570,38 @@ void runawayTakeoffTemporaryDisable(uint8_t disableFlag) // calculate the throttle stick percent - integer math is good enough here. -uint8_t calculateThrottlePercent(void) +// returns negative values for reversed thrust in 3D mode +int8_t calculateThrottlePercent(void) { uint8_t ret = 0; + int channelData = constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX); + if (featureIsEnabled(FEATURE_3D) && !IS_RC_MODE_ACTIVE(BOX3D) && !flight3DConfig()->switched_mode3d) { - - if ((rcData[THROTTLE] >= PWM_RANGE_MAX) || (rcData[THROTTLE] <= PWM_RANGE_MIN)) { - ret = 100; - } else { - if (rcData[THROTTLE] > (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { - ret = ((rcData[THROTTLE] - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle); - } else if (rcData[THROTTLE] < (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)) { - ret = ((rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - rcData[THROTTLE]) * 100) / (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - PWM_RANGE_MIN); - } + + if (channelData > (rxConfig()->midrc + flight3DConfig()->deadband3d_throttle)) { + ret = ((channelData - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle) * 100) / (PWM_RANGE_MAX - rxConfig()->midrc - flight3DConfig()->deadband3d_throttle); + } else if (channelData < (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle)) { + ret = -((rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - channelData) * 100) / (rxConfig()->midrc - flight3DConfig()->deadband3d_throttle - PWM_RANGE_MIN); } } else { - ret = constrain(((rcData[THROTTLE] - rxConfig()->mincheck) * 100) / (PWM_RANGE_MAX - rxConfig()->mincheck), 0, 100); + ret = constrain(((channelData - rxConfig()->mincheck) * 100) / (PWM_RANGE_MAX - rxConfig()->mincheck), 0, 100); + if (featureIsEnabled(FEATURE_3D) + && IS_RC_MODE_ACTIVE(BOX3D) + && flight3DConfig()->switched_mode3d) { + + ret = -ret; // 3D on a switch is active + } } return ret; } +uint8_t calculateThrottlePercentAbs(void) +{ + return ABS(calculateThrottlePercent()); +} + static bool airmodeIsActivated; bool isAirmodeActivated() @@ -629,7 +639,7 @@ bool processRx(timeUs_t currentTimeUs) failsafeUpdateState(); const throttleStatus_e throttleStatus = calculateThrottleStatus(); - const uint8_t throttlePercent = calculateThrottlePercent(); + const uint8_t throttlePercent = calculateThrottlePercentAbs(); const bool launchControlActive = isLaunchControlActive(); diff --git a/src/main/fc/core.h b/src/main/fc/core.h index 6d3aa646f7..2a1f1913d2 100644 --- a/src/main/fc/core.h +++ b/src/main/fc/core.h @@ -68,7 +68,8 @@ void updateArmingStatus(void); void taskMainPidLoop(timeUs_t currentTimeUs); bool isFlipOverAfterCrashActive(void); - +int8_t calculateThrottlePercent(void); +uint8_t calculateThrottlePercentAbs(void); void runawayTakeoffTemporaryDisable(uint8_t disableFlag); bool isAirmodeActivated(); timeUs_t getLastDisarmTimeUs(void); @@ -78,4 +79,3 @@ void resetTryingToArm(); void subTaskTelemetryPollSensors(timeUs_t currentTimeUs); bool isLaunchControlActive(void); - diff --git a/src/main/io/osd.c b/src/main/io/osd.c index fb6b4a9923..977b4d4b6e 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -764,7 +764,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_THROTTLE_POS: buff[0] = SYM_THR; buff[1] = SYM_THR1; - tfp_sprintf(buff + 2, "%3d", (constrain(rcData[THROTTLE], PWM_RANGE_MIN, PWM_RANGE_MAX) - PWM_RANGE_MIN) * 100 / (PWM_RANGE_MAX - PWM_RANGE_MIN)); + tfp_sprintf(buff + 2, "%3d", calculateThrottlePercent()); break; #if defined(USE_VTX_COMMON) diff --git a/src/test/unit/osd_unittest.cc b/src/test/unit/osd_unittest.cc index 7faf227c54..1161c2bae1 100644 --- a/src/test/unit/osd_unittest.cc +++ b/src/test/unit/osd_unittest.cc @@ -1079,4 +1079,5 @@ extern "C" { bool pidOsdAntiGravityActive(void) { return false; } bool failsafeIsActive(void) { return false; } bool gpsRescueIsConfigured(void) { return false; } + int8_t calculateThrottlePercent(void) { return 0; } }