mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-22 15:55:40 +03:00
sticks > pos_hold_deadband cleanup
This commit is contained in:
parent
c31acda500
commit
fab16951d5
5 changed files with 9 additions and 8 deletions
|
@ -189,7 +189,7 @@ static void updateArmingStatus(void)
|
||||||
|
|
||||||
/* CHECK: pitch / roll sticks centered when NAV_LAUNCH_MODE enabled */
|
/* CHECK: pitch / roll sticks centered when NAV_LAUNCH_MODE enabled */
|
||||||
if (isNavLaunchEnabled()) {
|
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);
|
ENABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED);
|
||||||
} else {
|
} else {
|
||||||
DISABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED);
|
DISABLE_ARMING_FLAG(ARMING_DISABLED_ROLLPITCH_NOT_CENTERED);
|
||||||
|
|
|
@ -90,6 +90,11 @@ bool areSticksInApModePosition(uint16_t ap_mode)
|
||||||
return ABS(rcCommand[ROLL]) < ap_mode && ABS(rcCommand[PITCH]) < 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)
|
throttleStatus_e calculateThrottleStatus(void)
|
||||||
{
|
{
|
||||||
const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle;
|
const uint16_t deadband3d_throttle = rcControlsConfig()->deadband3d_throttle;
|
||||||
|
|
|
@ -86,6 +86,7 @@ stickPositions_e getRcStickPositions(void);
|
||||||
bool checkStickPosition(stickPositions_e stickPos);
|
bool checkStickPosition(stickPositions_e stickPos);
|
||||||
|
|
||||||
bool areSticksInApModePosition(uint16_t ap_mode);
|
bool areSticksInApModePosition(uint16_t ap_mode);
|
||||||
|
bool areSticksDeflectedMoreThanPosHoldDeadband(void);
|
||||||
throttleStatus_e calculateThrottleStatus(void);
|
throttleStatus_e calculateThrottleStatus(void);
|
||||||
rollPitchStatus_e calculateRollPitchCenterStatus(void);
|
rollPitchStatus_e calculateRollPitchCenterStatus(void);
|
||||||
void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm);
|
void processRcStickPositions(throttleStatus_e throttleStatus, bool disarm_kill_switch, bool fixed_wing_auto_arm);
|
||||||
|
|
|
@ -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.
|
//allow to leave NAV_LAUNCH_MODE if it has being enabled as feature by moving sticks with low throttle.
|
||||||
if(feature(FEATURE_FW_LAUNCH)){
|
if(feature(FEATURE_FW_LAUNCH)){
|
||||||
throttleStatus_e throttleStatus = calculateThrottleStatus();
|
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;
|
return NAV_FSM_EVENT_SWITCH_TO_IDLE;
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -151,14 +151,9 @@ static inline bool isLaunchModeMaxTimeElapsed(float timeSinceLaunchMs)
|
||||||
return timeSinceLaunchMs >= navConfig()->fw.launch_timeout;
|
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)
|
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)
|
void applyFixedWingLaunchController(timeUs_t currentTimeUs)
|
||||||
|
|
Loading…
Add table
Add a link
Reference in a new issue