Mirror/inav
1
0
Fork 0
mirror of https://github.com/iNavFlight/inav.git synced 2025-07-26 01:35:35 +03:00
Code Issues Wiki Activity

inhibit failsafe on landing

Browse source
This commit is contained in:
breadoven 2024-06-12 23:12:48 +01:00
parent 9922e7d259
commit af8a561ed0
2 changed files with 15 additions and 14 deletions
Download patch file Download diff file Expand all files Collapse all files

16
src/main/flight/failsafe.c
View file

@ -84,7 +84,7 @@ PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig,
.failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // Time delay before Failsafe activated during WP mission (s) .failsafe_mission_delay = SETTING_FAILSAFE_MISSION_DELAY_DEFAULT, // Time delay before Failsafe activated during WP mission (s)
#ifdef USE_GPS_FIX_ESTIMATION #ifdef USE_GPS_FIX_ESTIMATION
.failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied .failsafe_gps_fix_estimation_delay = SETTING_FAILSAFE_GPS_FIX_ESTIMATION_DELAY_DEFAULT, // Time delay before Failsafe activated when GPS Fix estimation is allied
#endif #endif
); );
typedef enum { typedef enum {
@ -350,16 +350,16 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
} }
} }
// Inhibit Failsafe if emergency landing triggered manually // Inhibit Failsafe if emergency landing triggered manually or if landing is detected
if (posControl.flags.manualEmergLandActive) { if (posControl.flags.manualEmergLandActive || STATE(LANDING_DETECTED)) {
return FAILSAFE_PROCEDURE_NONE; return FAILSAFE_PROCEDURE_NONE;
} }
// Craft is closer than minimum failsafe procedure distance (if set to non-zero) // Craft is closer than minimum failsafe procedure distance (if set to non-zero)
// GPS must also be working, and home position set // GPS must also be working, and home position set
if (failsafeConfig()->failsafe_min_distance > 0 && if (failsafeConfig()->failsafe_min_distance > 0 &&
((sensors(SENSOR_GPS) && STATE(GPS_FIX)) ((sensors(SENSOR_GPS) && STATE(GPS_FIX))
#ifdef USE_GPS_FIX_ESTIMATION #ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX) || STATE(GPS_ESTIMATED_FIX)
#endif #endif
) && STATE(GPS_FIX_HOME)) { ) && STATE(GPS_FIX_HOME)) {
@ -429,8 +429,8 @@ void failsafeUpdateState(void)
#ifdef USE_GPS_FIX_ESTIMATION #ifdef USE_GPS_FIX_ESTIMATION
if ( checkGPSFixFailsafe() ) { if ( checkGPSFixFailsafe() ) {
reprocessState = true; reprocessState = true;
} else } else
#endif #endif
if (!receivingRxDataAndNotFailsafeMode) { if (!receivingRxDataAndNotFailsafeMode) {
if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) { if ((failsafeConfig()->failsafe_throttle_low_delay && (millis() > failsafeState.throttleLowPeriod)) || STATE(NAV_MOTOR_STOP_OR_IDLE)) {
// JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch // JustDisarm: throttle was LOW for at least 'failsafe_throttle_low_delay' seconds or waiting for launch
@ -499,7 +499,7 @@ void failsafeUpdateState(void)
} else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed } else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED; failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
reprocessState = true; reprocessState = true;
} }
#ifdef USE_GPS_FIX_ESTIMATION #ifdef USE_GPS_FIX_ESTIMATION
else { else {
if ( checkGPSFixFailsafe() ) { if ( checkGPSFixFailsafe() ) {

13
src/main/navigation/navigation.c
View file

@ -3321,10 +3321,6 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t d
*-----------------------------------------------------------*/ *-----------------------------------------------------------*/
void updateLandingStatus(timeMs_t currentTimeMs) void updateLandingStatus(timeMs_t currentTimeMs)
{ {
if (STATE(AIRPLANE) && !navConfig()->general.flags.disarm_on_landing) {
return; // no point using this with a fixed wing if not set to disarm
}
static timeMs_t lastUpdateTimeMs = 0; static timeMs_t lastUpdateTimeMs = 0;
if ((currentTimeMs - lastUpdateTimeMs) <= HZ2MS(100)) { // limit update to 100Hz if ((currentTimeMs - lastUpdateTimeMs) <= HZ2MS(100)) { // limit update to 100Hz
return; return;
@ -3354,8 +3350,13 @@ void updateLandingStatus(timeMs_t currentTimeMs)
ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED); ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
disarm(DISARM_LANDING); disarm(DISARM_LANDING);
} else if (!navigationInAutomaticThrottleMode()) { } else if (!navigationInAutomaticThrottleMode()) {
// for multirotor only - reactivate landing detector without disarm when throttle raised toward hover throttle if (STATE(AIRPLANE) && isFlightDetected()) {
landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue())); // Cancel landing detection flag if fixed wing redetected in flight
resetLandingDetector();
} else {
// For multirotor - reactivate landing detector without disarm when throttle raised toward hover throttle
landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
}
} }
} else if (isLandingDetected()) { } else if (isLandingDetected()) {
ENABLE_STATE(LANDING_DETECTED); ENABLE_STATE(LANDING_DETECTED);