1
0
Fork 0
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:
Darren Lines 2023-11-19 18:51:55 +00:00
parent 3740682352
commit f64639a060
3 changed files with 25 additions and 2 deletions

View file

@ -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