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

Merge pull request #8686 from breadoven/abo_manual_emerg_landing

Browse source 
Manual emergency landing FW and MR
This commit is contained in:
Paweł Spychalski 2023-01-27 16:11:05 +01:00 • committed by GitHub
commit bb79ad8f02
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
5 changed files with 86 additions and 11 deletions
Download patch file Download diff file Expand all files Collapse all files

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

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