diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 5dba486f88..b31c47cc99 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -189,7 +189,7 @@ static void updateArmingStatus(void) /* CHECK: pitch / roll sticks centered when NAV_LAUNCH_MODE enabled */ if (isNavLaunchEnabled()) { - if ((ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband)) { + if (areSticksDeflectedMoreThanPosHoldDeadband()) { ENABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED); } else { DISABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED); diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index c07fd60dc1..f387fc90bd 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -90,6 +90,11 @@ bool areSticksInApModePosition(uint16_t ap_mode) return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < ap_mode; } +bool areSticksDeflectedMoreThanPosHoldDeadband(void) +{ + return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband); +} + throttleStatus_e calculateThrottleStatus(void) { const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle; diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index 56c815d2e9..9ae7999a07 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -86,6 +86,7 @@ stickPositions_e getRcStickPositions(void); bool checkStickPosition(stickPositions_e stickPos); bool areSticksInApModePosition(uint16_t ap_mode); +bool areSticksDeflectedMoreThanPosHoldDeadband(void); throttleStatus_e calculateThrottleStatus(void); rollPitchStatus_e calculateRollPitchCenterStatus(void); void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm); diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 517534bfb0..7dc9e24e13 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1235,7 +1235,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_LAUNCH_WAIT(navigationF //allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle. if(feature(FEATURE_FW_LAUNCH)){ throttleStatus_e throttleStatus = calculateThrottleStatus(); - if ((throttleStatus == THROTTLE_LOW) && ((ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband))){ + if ((throttleStatus == THROTTLE_LOW) && (areSticksDeflectedMoreThanPosHoldDeadband())){ return NAV_FSM_EVENT_SWITCH_TO_IDLE; } } diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index dbb6fdd886..2bb763b9e1 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -151,14 +151,9 @@ static inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs) return timeSinceLaunchMs >= navConfig()->fw.launch_timeout; } -static inline bool isLaunchModeFinishedByPilot() -{ - return (ABS(rcCommand[ROLL]) > rcControlsConfig()->pos_hold_deadband) || (ABS(rcCommand[PITCH]) > rcControlsConfig()->pos_hold_deadband); -} - static inline bool isFixedWingLaunchCompleted(float timeSinceLaunchMs) { - return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (isLaunchModeFinishedByPilot())) || isFixedWingLaunchMaxAltitudeReached(); + return (isLaunchModeMaxTimeElapsed(timeSinceLaunchMs)) || ((isLaunchModeMinTimeElapsed(timeSinceLaunchMs)) && (areSticksDeflectedMoreThanPosHoldDeadband())) || isFixedWingLaunchMaxAltitudeReached(); } void applyFixedWingLaunchController(timeUs_t currentTimeUs)