Skip to content

Inhibit active failsafe procedures on landing detection #10135

New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Merged
breadoven merged 2 commits into from
Jun 21, 2024
Merged

Inhibit active failsafe procedures on landing detection #10135

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
2 commits
Select commit Hold shift + click to select a range
af8a561
inhibit failsafe on landing
breadoven Jun 12, 2024
68e7498
Update navigation.c
breadoven Jun 16, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
16 changes: 8 additions & 8 deletions src/main/flight/failsafe.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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)
#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
#endif
#endif
);

typedef enum {
Expand Down Expand Up @@ -350,16 +350,16 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void)
}
}

// Inhibit Failsafe if emergency landing triggered manually
if (posControl.flags.manualEmergLandActive) {
// Inhibit Failsafe if emergency landing triggered manually or if landing is detected
if (posControl.flags.manualEmergLandActive || STATE(LANDING_DETECTED)) {
return FAILSAFE_PROCEDURE_NONE;
}

// Craft is closer than minimum failsafe procedure distance (if set to non-zero)
// GPS must also be working, and home position set
if (failsafeConfig()->failsafe_min_distance > 0 &&
((sensors(SENSOR_GPS) && STATE(GPS_FIX))
#ifdef USE_GPS_FIX_ESTIMATION
((sensors(SENSOR_GPS) && STATE(GPS_FIX))
#ifdef USE_GPS_FIX_ESTIMATION
|| STATE(GPS_ESTIMATED_FIX)
#endif
) && STATE(GPS_FIX_HOME)) {
Expand Down Expand Up @@ -429,8 +429,8 @@ void failsafeUpdateState(void)
#ifdef USE_GPS_FIX_ESTIMATION
if ( checkGPSFixFailsafe() ) {
reprocessState = true;
} else
#endif
} else
#endif
if (!receivingRxDataAndNotFailsafeMode) {
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
Expand Down Expand Up @@ -499,7 +499,7 @@ void failsafeUpdateState(void)
} else if (failsafeChooseFailsafeProcedure() != FAILSAFE_PROCEDURE_NONE) { // trigger new failsafe procedure if changed
failsafeState.phase = FAILSAFE_RX_LOSS_DETECTED;
reprocessState = true;
}
}
#ifdef USE_GPS_FIX_ESTIMATION
else {
if ( checkGPSFixFailsafe() ) {
Expand Down
13 changes: 7 additions & 6 deletions src/main/navigation/navigation.c
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -3321,10 +3321,6 @@ void calculateFarAwayTarget(fpVector3_t * farAwayPos, int32_t bearing, int32_t d
*-----------------------------------------------------------*/
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;
if ((currentTimeMs - lastUpdateTimeMs) <= HZ2MS(100)) { // limit update to 100Hz
return;
Expand Down Expand Up @@ -3354,8 +3350,13 @@ void updateLandingStatus(timeMs_t currentTimeMs)
ENABLE_ARMING_FLAG(ARMING_DISABLED_LANDING_DETECTED);
disarm(DISARM_LANDING);
} else if (!navigationInAutomaticThrottleMode()) {
// for multirotor only - reactivate landing detector without disarm when throttle raised toward hover throttle
landingDetectorIsActive = rxGetChannelValue(THROTTLE) < (0.5 * (currentBatteryProfile->nav.mc.hover_throttle + getThrottleIdleValue()));
if (STATE(AIRPLANE) && isFlightDetected()) {
// Cancel landing detection flag if fixed wing redetected in flight
resetLandingDetector();
} else if (STATE(MULTIROTOR)) {
// 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()) {
ENABLE_STATE(LANDING_DETECTED);
Expand Down