diff --git a/src/main/common/maths.c b/src/main/common/maths.c index 6d973980cf..26b3fe21ba 100644 --- a/src/main/common/maths.c +++ b/src/main/common/maths.c @@ -142,6 +142,18 @@ int32_t applyDeadband(int32_t value, int32_t deadband) return value; } +int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int32_t max) +{ + if (ABS(value) < deadband) { + value = 0; + } else if (value > 0) { + value = scaleRange(value - deadband, 0, max - deadband, 0, max); + } else if (value < 0) { + value = scaleRange(value + deadband, min + deadband, 0, min, 0); + } + return value; +} + int32_t constrain(int32_t amt, int32_t low, int32_t high) { if (amt < low) diff --git a/src/main/common/maths.h b/src/main/common/maths.h index 19c906b8c1..6f2d9b1245 100644 --- a/src/main/common/maths.h +++ b/src/main/common/maths.h @@ -132,6 +132,7 @@ bool sensorCalibrationSolveForScale(sensorCalibrationState_t * state, float resu int gcd(int num, int denom); int32_t applyDeadband(int32_t value, int32_t deadband); +int32_t applyDeadbandRescaled(int32_t value, int32_t deadband, int32_t min, int32_t max); int32_t constrain(int32_t amt, int32_t low, int32_t high); float constrainf(float amt, float low, float high); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 5fe95c1544..57825a01cc 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -171,7 +171,7 @@ int16_t getAxisRcCommand(int16_t rawData, int16_t rate, int16_t deadband) int16_t stickDeflection; stickDeflection = constrain(rawData - PWM_RANGE_MIDDLE, -500, 500); - stickDeflection = applyDeadband(stickDeflection, deadband); + stickDeflection = applyDeadbandRescaled(stickDeflection, deadband, -500, 500); return rcLookup(stickDeflection, rate); } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index fd7e0c90d3..51d4983dcd 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2716,8 +2716,8 @@ static bool adjustPositionFromRCInput(void) } else { - const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband); - const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); + const int16_t rcPitchAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband, -500, 500); + const int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment); } diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 4ec12bc690..6501cc7ea2 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -106,7 +106,7 @@ void resetFixedWingAltitudeController(void) bool adjustFixedWingAltitudeFromRCInput(void) { - int16_t rcAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->alt_hold_deadband); + int16_t rcAdjustment = applyDeadbandRescaled(rcCommand[PITCH], rcControlsConfig()->alt_hold_deadband, -500, 500); if (rcAdjustment) { // set velocity proportional to stick movement @@ -281,7 +281,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) // Shift position according to pilot's ROLL input (up to max_manual_speed velocity) if (posControl.flags.isAdjustingPosition) { - int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); + int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); if (rcRollAdjustment) { float rcShiftY = rcRollAdjustment * navConfig()->general.max_manual_speed / 500.0f * trackingPeriod; @@ -295,7 +295,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) bool adjustFixedWingPositionFromRCInput(void) { - int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); + int16_t rcRollAdjustment = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); return (rcRollAdjustment); } @@ -650,7 +650,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, } if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition) - rcCommand[ROLL] = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); + rcCommand[ROLL] = applyDeadbandRescaled(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband, -500, 500); //if (navStateFlags & NAV_CTL_YAW) if ((navStateFlags & NAV_CTL_ALT) || (navStateFlags & NAV_CTL_POS)) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index debec2916c..8af05db4da 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -132,7 +132,7 @@ bool adjustMulticopterAltitudeFromRCInput(void) return true; } else { - const int16_t rcThrottleAdjustment = applyDeadband(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband); + const int16_t rcThrottleAdjustment = applyDeadbandRescaled(rcCommand[THROTTLE] - altHoldThrottleRCZero, rcControlsConfig()->alt_hold_deadband, -500, 500); if (rcThrottleAdjustment) { // set velocity proportional to stick movement float rcClimbRate;