mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-26 09:45:33 +03:00
Add param to delay landing in event of a Failsafe
This commit is contained in:
parent
3740682352
commit
f64639a060
3 changed files with 25 additions and 2 deletions
|
@ -155,6 +155,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig,
|
|||
.auto_disarm_delay = SETTING_NAV_AUTO_DISARM_DELAY_DEFAULT, // 2000 ms - time delay to disarm when auto disarm after landing enabled
|
||||
.rth_linear_descent_start_distance = SETTING_NAV_RTH_LINEAR_DESCENT_START_DISTANCE_DEFAULT,
|
||||
.cruise_yaw_rate = SETTING_NAV_CRUISE_YAW_RATE_DEFAULT, // 20dps
|
||||
.rth_fs_landing_delay = SETTING_NAV_RTH_FS_LANDING_DELAY_DEFAULT, // Delay before landing in FS. 0 = immedate landing
|
||||
},
|
||||
|
||||
// MC-specific
|
||||
|
@ -225,6 +226,7 @@ static navWapointHeading_t wpHeadingControl;
|
|||
navigationPosControl_t posControl;
|
||||
navSystemStatus_t NAV_Status;
|
||||
static bool landingDetectorIsActive;
|
||||
static timeMs_t landingDelay = 0;
|
||||
|
||||
EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
|
||||
|
||||
|
@ -1470,9 +1472,23 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
|
|||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
// If position ok OR within valid timeout - continue
|
||||
// Action delay before landing if in FS and option enabled
|
||||
bool pauseLanding = false;
|
||||
if (NAV_RTH_ALLOW_LANDING_FS_ONLY && FLIGHT_MODE(FAILSAFE_MODE) && navConfig()->general.rth_fs_landing_delay > 0) {
|
||||
if (landingDelay == 0)
|
||||
landingDelay = millis() + S2MS(navConfig()->general.rth_fs_landing_delay);
|
||||
|
||||
batteryState_e batteryState = getBatteryState();
|
||||
|
||||
if (millis() < landingDelay && batteryState != BATTERY_WARNING && batteryState != BATTERY_CRITICAL)
|
||||
pauseLanding = true;
|
||||
else
|
||||
landingDelay = 0;
|
||||
}
|
||||
|
||||
// If landing is not temporarily paused (FS only), position ok, OR within valid timeout - continue
|
||||
// Wait until target heading is reached for MR (with 15 deg margin for error), or continue for Fixed Wing
|
||||
if ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY)) {
|
||||
if (!pauseLanding && ((ABS(wrap_18000(posControl.rthState.homePosition.heading - posControl.actualState.yaw)) < DEGREES_TO_CENTIDEGREES(15)) || STATE(FIXED_WING_LEGACY))) {
|
||||
resetLandingDetector(); // force reset landing detector just in case
|
||||
updateClimbRateToAltitudeController(0, 0, ROC_TO_ALT_RESET);
|
||||
return navigationRTHAllowsLanding() ? NAV_FSM_EVENT_SUCCESS : NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME; // success = land
|
||||
|
|
Loading…
Add table
Add a link
Reference in a new issue