diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index eff24c0c87..43e891b43e 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1972,6 +1972,9 @@ 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); + /* * Process states only for POSHOLD. In any other case we go back to old routines */ @@ -1984,9 +1987,6 @@ static bool adjustPositionFromRCInput(void) NAV_Status.state == MW_NAV_STATE_HOLD_INFINIT ) ) { - const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband); - const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); - /* * Case one, when we order to brake (sticks to the center) and we are moving above threshold * Speed is above 1m/s and sticks are centered @@ -2077,7 +2077,7 @@ static bool adjustPositionFromRCInput(void) } - retValue = adjustMulticopterPositionFromRCInput(); + retValue = adjustMulticopterPositionFromRCInput(rcPitchAdjustment, rcRollAdjustment); } return retValue; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index c732ca904d..74d4e7a25e 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -274,11 +274,8 @@ void resetMulticopterPositionController(void) } } -bool adjustMulticopterPositionFromRCInput(void) +bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment) { - const int16_t rcPitchAdjustment = applyDeadband(rcCommand[PITCH], rcControlsConfig()->pos_hold_deadband); - const int16_t rcRollAdjustment = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); - if (rcPitchAdjustment || rcRollAdjustment) { // If mode is GPS_CRUISE, move target position, otherwise POS controller will passthru the RC input to ANGLE PID if (navConfig()->general.flags.user_control_mode == NAV_GPS_CRUISE) { diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index e837ad7617..05300ebebe 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -390,7 +390,7 @@ void resetMulticopterHeadingController(void); bool adjustMulticopterAltitudeFromRCInput(void); bool adjustMulticopterHeadingFromRCInput(void); -bool adjustMulticopterPositionFromRCInput(void); +bool adjustMulticopterPositionFromRCInput(int16_t rcPitchAdjustment, int16_t rcRollAdjustment); void applyMulticopterNavigationController(navigationFSMStateFlags_t navStateFlags, timeUs_t currentTimeUs);