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

Merge branch 'master' into abo_failsafe_emergency_landing

Browse source
This commit is contained in:
breadoven 2021-09-14 15:32:03 +01:00 • committed by GitHub
commit c81cf30d94
No known key found for this signature in database
GPG key ID: 4AEE18F83AFDEB23
60 changed files with 542 additions and 769 deletions
Download patch file Download diff file Expand all files Collapse all files

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

@ -199,7 +199,7 @@ navigationPosControl_t posControl;
navSystemStatus_t NAV_Status;
EXTENDED_FASTRAM multicopterPosXyCoefficients_t multicopterPosXyCoefficients;
#if defined(NAV_BLACKBOX)
// Blackbox states
int16_t navCurrentState;
int16_t navActualVelocity[3];
int16_t navDesiredVelocity[3];
@ -212,7 +212,7 @@ uint16_t navFlags;
uint16_t navEPH;
uint16_t navEPV;
int16_t navAccNEU[3];
#endif
//End of blackbox states
static fpVector3_t * rthGetHomeTargetPosition(rthTargetMode_e mode);
static void updateDesiredRTHAltitude(void);
@ -774,12 +774,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_STATE_WAYPOINT_FINISHED] = {
.persistentId = NAV_PERSISTENT_ID_WAYPOINT_FINISHED,
.onEntry = navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED,
.timeoutMs = 0,
.timeoutMs = 10,
.stateFlags = NAV_CTL_ALT | NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_REQUIRE_MAGHOLD | NAV_REQUIRE_THRTILT | NAV_AUTO_WP | NAV_AUTO_WP_DONE,
.mapToFlightModes = NAV_WP_MODE | NAV_ALTHOLD_MODE,
.mwState = MW_NAV_STATE_WP_ENROUTE,
.mwError = MW_NAV_ERROR_FINISH,
.onEvent = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE,
@ -803,7 +804,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS,
[NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_IDLE, // ALTHOLD also bails out from emergency (to IDLE, AltHold will take over from there)
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
}
},
@ -819,7 +822,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = {
[NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // re-process the state
[NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_FINISHED,
[NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE,
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_IDLE, // ALTHOLD also bails out from emergency (to IDLE, AltHold will take over from there)
[NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE,
[NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE,
}
},
@ -1105,8 +1110,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
{
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) {
if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || !STATE(GPS_FIX_HOME)) {
// Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing
// Relevant to failsafe forced RTH only. Switched RTH blocked in selectNavEventFromBoxModeInput if sensors unavailanle.
// If we are in dead-reckoning mode - also fail, since coordinates may be unreliable
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
@ -1162,8 +1168,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
return NAV_FSM_EVENT_SUCCESS; // NAV_STATE_RTH_CLIMB_TO_SAFE_ALT
}
}
/* Position sensor failure timeout - land */
else if (checkForPositionSensorTimeout()) {
/* Position sensor failure timeout - land. Land immediately if failsafe RTH and timeout disabled (set to 0) */
else if (checkForPositionSensorTimeout() || (!navConfig()->general.pos_failure_timeout && posControl.flags.forcedRTHActivated)) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
/* No valid POS sensor but still within valid timeout - wait */
@ -1330,7 +1336,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_ABOVE_HOME(na
{
UNUSED(previousState);
if (!(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()))
if (posControl.flags.estHeadingStatus == EST_NONE || !(validateRTHSanityChecker() || (posControl.flags.estPosStatus >= EST_USABLE) || !checkForPositionSensorTimeout()))
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
fpVector3_t * tmpHomePos = rthGetHomeTargetPosition(RTH_HOME_FINAL_HOVER);
@ -1369,8 +1375,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
return NAV_FSM_EVENT_SUCCESS;
}
else {
if (!validateRTHSanityChecker()) {
// Continue to check for RTH sanity during landing
/* If position sensors unavailable - land immediately (wait for timeout on GPS)
* Continue to check for RTH sanity during landing */
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout() ||!validateRTHSanityChecker()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
@ -1565,8 +1572,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
UNREACHABLE();
}
}
/* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
else if (checkForPositionSensorTimeout()) {
/* If position sensors unavailable - land immediately (wait for timeout on GPS) */
else if (checkForPositionSensorTimeout() || (posControl.flags.estHeadingStatus == EST_NONE)) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
else {
@ -1606,6 +1613,11 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_HOLD_TIME(navi
{
UNUSED(previousState);
/* If position sensors unavailable - land immediately (wait for timeout on GPS) */
if (posControl.flags.estHeadingStatus == EST_NONE || checkForPositionSensorTimeout()) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
timeMs_t currentTime = millis();
if(posControl.waypointList[posControl.activeWaypointIndex].p1 <= 0)
@ -1950,12 +1962,11 @@ void updateActualHorizontalPositionAndVelocity(bool estPosValid, bool estVelVali
posControl.flags.horizontalPositionDataNew = 0;
}
#if defined(NAV_BLACKBOX)
//Update blackbox data
navLatestActualPosition[X] = newX;
navLatestActualPosition[Y] = newY;
navActualVelocity[X] = constrain(newVelX, -32678, 32767);
navActualVelocity[Y] = constrain(newVelY, -32678, 32767);
#endif
}
/*-----------------------------------------------------------
@ -2005,10 +2016,9 @@ void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, flo
posControl.actualState.surfaceMin = -1;
}
#if defined(NAV_BLACKBOX)
//Update blackbox data
navLatestActualPosition[Z] = navGetCurrentActualPositionAndVelocity()->pos.z;
navActualVelocity[Z] = constrain(navGetCurrentActualPositionAndVelocity()->vel.z, -32678, 32767);
#endif
}
/*-----------------------------------------------------------
@ -3066,7 +3076,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
{
const timeUs_t currentTimeUs = micros();
#if defined(NAV_BLACKBOX)
//Updata blackbox data
navFlags = 0;
if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0);
if (posControl.flags.estAglStatus == EST_TRUSTED) navFlags |= (1 << 1);
@ -3076,7 +3086,6 @@ void applyWaypointNavigationAndAltitudeHold(void)
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
#endif
if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5);
#endif
// Reset all navigation requests - NAV controllers will set them if necessary
DISABLE_STATE(NAV_MOTOR_STOP_OR_IDLE);
@ -3111,8 +3120,7 @@ void applyWaypointNavigationAndAltitudeHold(void)
if (posControl.flags.verticalPositionDataConsumed)
posControl.flags.verticalPositionDataNew = 0;
#if defined(NAV_BLACKBOX)
//Update blackbox data
if (posControl.flags.isAdjustingPosition) navFlags |= (1 << 6);
if (posControl.flags.isAdjustingAltitude) navFlags |= (1 << 7);
if (posControl.flags.isAdjustingHeading) navFlags |= (1 << 8);
@ -3120,7 +3128,6 @@ void applyWaypointNavigationAndAltitudeHold(void)
navTargetPosition[X] = lrintf(posControl.desiredState.pos.x);
navTargetPosition[Y] = lrintf(posControl.desiredState.pos.y);
navTargetPosition[Z] = lrintf(posControl.desiredState.pos.z);
#endif
}
/*-----------------------------------------------------------
@ -3181,6 +3188,18 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
/* Keep Emergency landing mode active once triggered. Is deactivated when landing in progress if WP or RTH cancelled
* or position sensors working again or if Manual or Althold modes selected.
* Remains active if landing finished regardless of sensor status or if WP or RTH modes still selected */
if (navigationIsExecutingAnEmergencyLanding()) {
if (!(canActivateNavigation && canActivateAltHold && STATE(GPS_FIX_HOME)) &&
!IS_RC_MODE_ACTIVE(BOXMANUAL) &&
!(IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && canActivateAltHold) &&
(IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVRTH))) {
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
}
}
// Keep canActivateWaypoint flag at FALSE if there is no mission loaded
// Also block WP mission if we are executing RTH
if (!isWaypointMissionValid() || isExecutingRTH) {
@ -3469,9 +3488,8 @@ void updateWaypointsAndNavigationMode(void)
// Map navMode back to enabled flight modes
switchNavigationFlightModes();
#if defined(NAV_BLACKBOX)
//Update Blackbox data
navCurrentState = (int16_t)posControl.navPersistentId;
#endif
}
/*-----------------------------------------------------------