diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 097e5c3a99..ab48fba2c6 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -346,6 +346,11 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) } } + // Inhibit Failsafe if emergency landing triggered manually + if (posControl.flags.manualEmergLandActive) { + 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 && diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 2b469583d4..e164b33ab5 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1774,9 +1774,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_INITIALIZE(navigationFSMState_t previousState) { - // TODO: UNUSED(previousState); + if ((posControl.flags.estPosStatus >= EST_USABLE)) { + resetPositionController(); + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY); + } + // Emergency landing MAY use common altitude controller if vertical position is valid - initialize it // Make sure terrain following is not enabled resetAltitudeController(false); @@ -1788,6 +1792,14 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_EMERGENCY_LANDING_IN_PR { UNUSED(previousState); + // Reset target position if too far away for some reason, e.g. GPS recovered since start landing. + if (posControl.flags.estPosStatus >= EST_USABLE) { + float targetPosLimit = STATE(MULTIROTOR) ? 2000.0f : navConfig()->fw.loiter_radius * 2.0f; + if (calculateDistanceToDestination(&posControl.desiredState.pos) > targetPosLimit) { + setDesiredPosition(&navGetCurrentActualPositionAndVelocity()->pos, 0, NAV_POS_UPDATE_XY); + } + } + if (STATE(LANDING_DETECTED)) { return NAV_FSM_EVENT_SUCCESS; } @@ -3421,7 +3433,10 @@ bool isNavHoldPositionActive(void) return isLastMissionWaypoint() || NAV_Status.state == MW_NAV_STATE_HOLD_TIMED; } // RTH mode (spiral climb and Home positions but excluding RTH Trackback point positions) and POSHOLD mode - return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) || FLIGHT_MODE(NAV_POSHOLD_MODE); + // Also hold position during emergency landing if position valid + return (FLIGHT_MODE(NAV_RTH_MODE) && !posControl.flags.rthTrackbackActive) || + FLIGHT_MODE(NAV_POSHOLD_MODE) || + navigationIsExecutingAnEmergencyLanding(); } float getActiveWaypointSpeed(void) @@ -3520,6 +3535,7 @@ void applyWaypointNavigationAndAltitudeHold(void) // If we are disarmed, abort forced RTH or Emergency Landing posControl.flags.forcedRTHActivated = false; posControl.flags.forcedEmergLandingActivated = false; + posControl.flags.manualEmergLandActive = false; // ensure WP missions always restart from first waypoint after disarm posControl.activeWaypointIndex = posControl.startWpIndex; // Reset RTH trackback @@ -3596,6 +3612,41 @@ static bool isWaypointMissionValid(void) return posControl.waypointListValid && (posControl.waypointCount > 0); } +static void checkManualEmergencyLandingControl(void) +{ + static timeMs_t timeout = 0; + static int8_t counter = 0; + static bool toggle; + timeMs_t currentTimeMs = millis(); + + if (timeout && currentTimeMs > timeout) { + timeout += 1000; + counter -= counter ? 1 : 0; + if (!counter) { + timeout = 0; + } + } + if (IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD)) { + if (!timeout && toggle) { + timeout = currentTimeMs + 4000; + } + counter += toggle; + toggle = false; + } else { + toggle = true; + } + + // Emergency landing toggled ON or OFF after 5 cycles of Poshold mode @ 1Hz minimum rate + if (counter >= 5) { + counter = 0; + posControl.flags.manualEmergLandActive = !posControl.flags.manualEmergLandActive; + + if (!posControl.flags.manualEmergLandActive) { + navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); + } + } +} + static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) { static bool canActivateWaypoint = false; @@ -3621,8 +3672,12 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) posControl.flags.rthTrackbackActive = isExecutingRTH; } - /* Emergency landing triggered by failsafe when Failsafe procedure set to Landing */ - if (posControl.flags.forcedEmergLandingActivated) { + /* Emergency landing controlled manually by rapid switching of Poshold mode. + * Landing toggled ON or OFF for each Poshold activation sequence */ + checkManualEmergencyLandingControl(); + + /* Emergency landing triggered by failsafe Landing or manually initiated */ + if (posControl.flags.forcedEmergLandingActivated || posControl.flags.manualEmergLandActive) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; } diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 299131a5c1..e77ef8ae4e 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -752,8 +752,6 @@ bool isFixedWingLandingDetected(void) *-----------------------------------------------------------*/ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) { - rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); - rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); rcCommand[THROTTLE] = currentBatteryProfile->failsafe_throttle; if (posControl.flags.estAltStatus >= EST_USABLE) { @@ -765,6 +763,18 @@ void applyFixedWingEmergencyLandingController(timeUs_t currentTimeUs) } else { rcCommand[PITCH] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_pitch_angle, pidProfile()->max_angle_inclination[FD_PITCH]); } + + if (posControl.flags.estPosStatus >= EST_USABLE) { // Hold position if possible + applyFixedWingPositionController(currentTimeUs); + int16_t rollCorrection = constrain(posControl.rcAdjustment[ROLL], + -DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle), + DEGREES_TO_DECIDEGREES(navConfig()->fw.max_bank_angle)); + rcCommand[ROLL] = pidAngleToRcCommand(rollCorrection, pidProfile()->max_angle_inclination[FD_ROLL]); + rcCommand[YAW] = 0; + } else { + rcCommand[ROLL] = pidAngleToRcCommand(failsafeConfig()->failsafe_fw_roll_angle, pidProfile()->max_angle_inclination[FD_ROLL]); + rcCommand[YAW] = -pidRateToRcCommand(failsafeConfig()->failsafe_fw_yaw_rate, currentControlRateProfile->stabilized.rates[FD_YAW]); + } } /*----------------------------------------------------------- diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index 7c2b3067ac..6e9585b4f0 100644 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -814,8 +814,6 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) static timeUs_t previousTimePositionUpdate = 0; /* Attempt to stabilise */ - rcCommand[ROLL] = 0; - rcCommand[PITCH] = 0; rcCommand[YAW] = 0; if ((posControl.flags.estAltStatus < EST_USABLE)) { @@ -852,6 +850,14 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) // Update throttle controller rcCommand[THROTTLE] = posControl.rcAdjustment[THROTTLE]; + + // Hold position if possible + if ((posControl.flags.estPosStatus >= EST_USABLE)) { + applyMulticopterPositionController(currentTimeUs); + } else { + rcCommand[ROLL] = 0; + rcCommand[PITCH] = 0; + } } /*----------------------------------------------------------- diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 26f1a8fb04..0598467d28 100644 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -104,14 +104,13 @@ typedef struct navigationFlags_s { bool forcedRTHActivated; bool forcedEmergLandingActivated; - bool wpMissionPlannerActive; // Activation status of WP mission planner - /* Landing detector */ bool resetLandingDetector; + bool wpMissionPlannerActive; // Activation status of WP mission planner bool rthTrackbackActive; // Activation status of RTH trackback - bool wpTurnSmoothingActive; // Activation status WP turn smoothing + bool manualEmergLandActive; // Activation status of manual emergency landing } navigationFlags_t; typedef struct {