diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index bb8f15dc8ba..7da742829c5 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -137,17 +137,17 @@ bool adjustMulticopterAltitudeFromRCInput(void) return true; } else { - const int16_t rcThrottleAdjustment = applyDeadbandRescaled(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500); + const uint8_t deadband = rcControlsConfig()->alt_hold_deadband; + const int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altHoldThrottleRCZero, deadband); if (rcThrottleAdjustment) { /* Set velocity proportional to stick movement * Scale from altHoldThrottleRCZero to maxthrottle or minthrottle to altHoldThrottleRCZero */ - // Calculate max up or min down limit value scaled for deadband - int16_t limitValue = rcThrottleAdjustment > 0 ? getMaxThrottle() : getThrottleIdleValue(); - limitValue = applyDeadbandRescaled(limitValue - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500); + int16_t controlRange = -deadband; + controlRange += rcThrottleAdjustment > 0 ? getMaxThrottle() - altHoldThrottleRCZero : altHoldThrottleRCZero - getThrottleIdleValue(); - int16_t rcClimbRate = ABS(rcThrottleAdjustment) * navConfig()->mc.max_manual_climb_rate / limitValue; + const int16_t rcClimbRate = rcThrottleAdjustment * navConfig()->mc.max_manual_climb_rate / controlRange; updateClimbRateToAltitudeController(rcClimbRate, 0, ROC_TO_ALT_CONSTANT); return true;