mirror of
https://github.com/iNavFlight/inav.git
synced 2025-07-24 00:35:34 +03:00
Merge branch 'development' into te_no_homereset_on_failsafe
This commit is contained in:
Tim Eckel
GitHub
commit
3050886f69
242 changed files with 5499 additions and 4733 deletions
|
|
@ -54,6 +54,15 @@
|
|||
#include "sensors/boardalignment.h"
|
||||
|
||||
|
||||
// Multirotors:
|
||||
#define MR_RTH_CLIMB_OVERSHOOT_CM 100 // target this amount of cm *above* the target altitude to ensure it is actually reached (Vz > 0 at target alt)
|
||||
#define MR_RTH_CLIMB_MARGIN_MIN_CM 100 // start cruising home this amount of cm *before* reaching the cruise altitude (while continuing the ascend)
|
||||
#define MR_RTH_CLIMB_MARGIN_PERCENT 15 // on high RTH altitudes use even bigger margin - percent of the altitude set
|
||||
// Planes:
|
||||
#define FW_RTH_CLIMB_OVERSHOOT_CM 100
|
||||
#define FW_RTH_CLIMB_MARGIN_MIN_CM 100
|
||||
#define FW_RTH_CLIMB_MARGIN_PERCENT 15
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Compatibility for home position
|
||||
*-----------------------------------------------------------*/
|
||||
|
|
@ -696,11 +705,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(nav
|
|||
resetGCSFlags();
|
||||
}
|
||||
|
||||
// If we enable terrain mode and surface offset is not set yet - do it
|
||||
if (posControl.flags.hasValidSurfaceSensor && posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface < 0) {
|
||||
setDesiredSurfaceOffset(posControl.actualState.surface);
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
|
|
@ -779,11 +783,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(
|
|||
resetGCSFlags();
|
||||
}
|
||||
|
||||
// If we enable terrain mode and surface offset is not set yet - do it
|
||||
if (posControl.flags.hasValidSurfaceSensor && posControl.flags.isTerrainFollowEnabled && posControl.desiredState.surface < 0) {
|
||||
setDesiredSurfaceOffset(posControl.actualState.surface);
|
||||
}
|
||||
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
|
||||
|
|
@ -791,7 +790,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
{
|
||||
navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState);
|
||||
|
||||
if (!posControl.flags.hasValidHeadingSensor || !posControl.flags.hasValidAltitudeSensor || !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
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
|
@ -802,7 +801,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati
|
|||
}
|
||||
|
||||
// If we have valid position sensor or configured to ignore it's loss at initial stage - continue
|
||||
if (posControl.flags.hasValidPositionSensor || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
// Reset altitude and position controllers if necessary
|
||||
if ((prevFlags & NAV_CTL_POS) == 0) {
|
||||
resetPositionController();
|
||||
|
|
@ -854,15 +853,15 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (!posControl.flags.hasValidHeadingSensor) {
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE)) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
// If we have valid pos sensor OR we are configured to ignore GPS loss
|
||||
if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) || !checkForPositionSensorTimeout() || navConfig()->general.flags.rth_climb_ignore_emerg) {
|
||||
const float rthAltitudeMargin = STATE(FIXED_WING) ?
|
||||
MAX(100.0f, 0.10f * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)) : // Airplane: 10% of target altitude but no less than 1m
|
||||
MAX( 50.0f, 0.05f * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)); // Copters: 5% of target altitude but no less than 50cm
|
||||
MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (FW_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)) : // Airplane
|
||||
MAX(MR_RTH_CLIMB_MARGIN_MIN_CM, (MR_RTH_CLIMB_MARGIN_PERCENT/100.0) * ABS(posControl.homeWaypointAbove.pos.V.Z - posControl.homePosition.pos.V.Z)); // Copters
|
||||
|
||||
if (((posControl.actualState.pos.V.Z - posControl.homeWaypointAbove.pos.V.Z) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) {
|
||||
// Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance
|
||||
|
|
@ -882,14 +881,22 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n
|
|||
|
||||
// Climb to safe altitude and turn to correct direction
|
||||
if (STATE(FIXED_WING)) {
|
||||
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z);
|
||||
t_fp_vector pos = posControl.homeWaypointAbove.pos;
|
||||
pos.V.Z += FW_RTH_CLIMB_OVERSHOOT_CM;
|
||||
|
||||
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z);
|
||||
}
|
||||
else {
|
||||
// Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach
|
||||
// it in a reasonable time. Immediately after we finish this phase - target the original altitude.
|
||||
t_fp_vector pos = posControl.homeWaypointAbove.pos;
|
||||
pos.V.Z += MR_RTH_CLIMB_OVERSHOOT_CM;
|
||||
|
||||
if (navConfig()->general.flags.rth_tail_first) {
|
||||
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
|
||||
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING_TAIL_FIRST);
|
||||
}
|
||||
else {
|
||||
setDesiredPosition(&posControl.homeWaypointAbove.pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
|
||||
setDesiredPosition(&pos, 0, NAV_POS_UPDATE_Z | NAV_POS_UPDATE_BEARING);
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -906,12 +913,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (!posControl.flags.hasValidHeadingSensor) {
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE)) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
// If we have position sensor - continue home
|
||||
if (posControl.flags.hasValidPositionSensor) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
if (isWaypointReached(&posControl.homeWaypointAbove, true)) {
|
||||
// Successfully reached position target - update XYZ-position
|
||||
setDesiredPosition(&posControl.homeWaypointAbove.pos, posControl.homeWaypointAbove.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
|
|
@ -946,12 +953,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HOVER_PRIOR_TO_LAND
|
|||
{
|
||||
UNUSED(previousState);
|
||||
|
||||
if (!posControl.flags.hasValidHeadingSensor) {
|
||||
if ((posControl.flags.estHeadingStatus == EST_NONE)) {
|
||||
return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING;
|
||||
}
|
||||
|
||||
// If position ok OR within valid timeout - continue
|
||||
if (posControl.flags.hasValidPositionSensor || !checkForPositionSensorTimeout()) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) || !checkForPositionSensorTimeout()) {
|
||||
// Wait until target heading is reached (with 15 deg margin for error)
|
||||
if (STATE(FIXED_WING)) {
|
||||
resetLandingDetector();
|
||||
|
|
@ -997,7 +1004,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_LANDING(navigationF
|
|||
float descentVelLimited = 0;
|
||||
|
||||
// A safeguard - if surface altitude sensors is available and it is reading < 50cm altitude - drop to low descend speed
|
||||
if (posControl.flags.hasValidSurfaceSensor && posControl.actualState.surface < 50.0f) {
|
||||
if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && posControl.actualState.surface < 50.0f) {
|
||||
// land_descent_rate == 200 : descend speed = 30 cm/s, gentle touchdown
|
||||
// Do not allow descent velocity slower than -30cm/s so the landing detector works.
|
||||
descentVelLimited = MIN(-0.15f * navConfig()->general.land_descent_rate, -30.0f);
|
||||
|
|
@ -1080,7 +1087,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_IN_PROGRESS(na
|
|||
UNUSED(previousState);
|
||||
|
||||
// If no position sensor available - land immediately
|
||||
if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
const bool isDoingRTH = (posControl.waypointList[posControl.activeWaypointIndex].action == NAV_WP_ACTION_RTH);
|
||||
|
||||
switch (posControl.waypointList[posControl.activeWaypointIndex].action) {
|
||||
|
|
@ -1164,7 +1171,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_FINISHED(navig
|
|||
UNUSED(previousState);
|
||||
|
||||
// If no position sensor available - land immediately
|
||||
if (posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE)) {
|
||||
return NAV_FSM_EVENT_NONE;
|
||||
}
|
||||
/* No pos sensor available for NAV_WAIT_FOR_GPS_TIMEOUT_MS - land */
|
||||
|
|
@ -1435,7 +1442,7 @@ bool isThrustFacingDownwards(void)
|
|||
bool checkForPositionSensorTimeout(void)
|
||||
{
|
||||
if (navConfig()->general.pos_failure_timeout) {
|
||||
if (!posControl.flags.hasValidPositionSensor && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
|
||||
if ((posControl.flags.estPosStatue == EST_NONE) && ((millis() - posControl.lastValidPositionTimeMs) > (1000 * navConfig()->general.pos_failure_timeout))) {
|
||||
return true;
|
||||
}
|
||||
else {
|
||||
|
|
@ -1451,7 +1458,7 @@ bool checkForPositionSensorTimeout(void)
|
|||
/*-----------------------------------------------------------
|
||||
* Processes an update to XY-position and velocity
|
||||
*-----------------------------------------------------------*/
|
||||
void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX, float newY, float newVelX, float newVelY)
|
||||
void updateActualHorizontalPositionAndVelocity(bool estimateValid, float newX, float newY, float newVelX, float newVelY)
|
||||
{
|
||||
posControl.actualState.pos.V.X = newX;
|
||||
posControl.actualState.pos.V.Y = newY;
|
||||
|
|
@ -1460,17 +1467,18 @@ void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX,
|
|||
posControl.actualState.vel.V.Y = newVelY;
|
||||
posControl.actualState.velXY = sqrtf(sq(newVelX) + sq(newVelY));
|
||||
|
||||
posControl.flags.hasValidPositionSensor = hasValidSensor;
|
||||
posControl.flags.hasValidHeadingSensor = isImuHeadingValid();
|
||||
|
||||
if (hasValidSensor) {
|
||||
if (estimateValid) {
|
||||
posControl.flags.estPosStatue = EST_TRUSTED;
|
||||
posControl.flags.horizontalPositionDataNew = 1;
|
||||
posControl.lastValidPositionTimeMs = millis();
|
||||
}
|
||||
else {
|
||||
posControl.flags.estPosStatue = EST_NONE;
|
||||
posControl.flags.horizontalPositionDataNew = 0;
|
||||
}
|
||||
|
||||
posControl.flags.estHeadingStatus = isImuHeadingValid() ? EST_TRUSTED : EST_NONE;
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navLatestActualPosition[X] = newX;
|
||||
navLatestActualPosition[Y] = newY;
|
||||
|
|
@ -1482,51 +1490,37 @@ void updateActualHorizontalPositionAndVelocity(bool hasValidSensor, float newX,
|
|||
/*-----------------------------------------------------------
|
||||
* Processes an update to Z-position and velocity
|
||||
*-----------------------------------------------------------*/
|
||||
void updateActualAltitudeAndClimbRate(bool hasValidSensor, float newAltitude, float newVelocity)
|
||||
void updateActualAltitudeAndClimbRate(bool estimateValid, float newAltitude, float newVelocity, float surfaceDistance, float surfaceVelocity, navigationEstimateStatus_e surfaceStatus)
|
||||
{
|
||||
posControl.actualState.pos.V.Z = newAltitude;
|
||||
posControl.actualState.vel.V.Z = newVelocity;
|
||||
|
||||
posControl.flags.hasValidAltitudeSensor = hasValidSensor;
|
||||
posControl.actualState.surface = surfaceDistance;
|
||||
posControl.actualState.surfaceVel = surfaceVelocity;
|
||||
|
||||
// Update altitude that would be used when executing RTH
|
||||
if (hasValidSensor) {
|
||||
if (estimateValid) {
|
||||
updateDesiredRTHAltitude();
|
||||
|
||||
// If we acquired new surface reference - changing from NONE/USABLE -> TRUSTED
|
||||
if ((surfaceStatus == EST_TRUSTED) && (posControl.flags.estSurfaceStatus != EST_TRUSTED)) {
|
||||
// If we are in terrain-following modes - signal that we should update the surface tracking setpoint
|
||||
// NONE/USABLE means that we were flying blind, now we should lock to surface
|
||||
//updateSurfaceTrackingSetpoint();
|
||||
}
|
||||
|
||||
posControl.flags.estSurfaceStatus = surfaceStatus; // Could be TRUSTED or USABLE
|
||||
posControl.flags.estAltStatus = EST_TRUSTED;
|
||||
posControl.flags.verticalPositionDataNew = 1;
|
||||
posControl.lastValidAltitudeTimeMs = millis();
|
||||
}
|
||||
else {
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.estSurfaceStatus = EST_NONE;
|
||||
posControl.flags.verticalPositionDataNew = 0;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navLatestActualPosition[Z] = constrain(newAltitude, -32678, 32767);
|
||||
navActualVelocity[Z] = constrain(newVelocity, -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Processes an update to surface distance
|
||||
*-----------------------------------------------------------*/
|
||||
void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, float surfaceVelocity)
|
||||
{
|
||||
posControl.actualState.surface = surfaceDistance;
|
||||
posControl.actualState.surfaceVel = surfaceVelocity;
|
||||
|
||||
// Update validity
|
||||
// Update validity
|
||||
if (hasValidSensor) {
|
||||
posControl.flags.hasValidSurfaceSensor = true;
|
||||
posControl.flags.surfaceDistanceDataNew = 1;
|
||||
}
|
||||
else {
|
||||
posControl.flags.hasValidSurfaceSensor = false;
|
||||
posControl.flags.surfaceDistanceDataNew = 0;
|
||||
}
|
||||
|
||||
// Update minimum surface distance (landing detection threshold)
|
||||
if (ARMING_FLAG(ARMED)) {
|
||||
if (hasValidSensor && posControl.actualState.surface > 0) {
|
||||
if ((posControl.flags.estSurfaceStatus == EST_TRUSTED) && posControl.actualState.surface > 0) {
|
||||
if (posControl.actualState.surfaceMin > 0) {
|
||||
posControl.actualState.surfaceMin = MIN(posControl.actualState.surfaceMin, posControl.actualState.surface);
|
||||
}
|
||||
|
|
@ -1540,7 +1534,8 @@ void updateActualSurfaceDistance(bool hasValidSensor, float surfaceDistance, flo
|
|||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navActualSurface = surfaceDistance;
|
||||
navLatestActualPosition[Z] = constrain(newAltitude, -32678, 32767);
|
||||
navActualVelocity[Z] = constrain(newVelocity, -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
|
|
@ -1723,7 +1718,7 @@ void updateHomePosition(void)
|
|||
{
|
||||
// Disarmed and have a valid position, constantly update home
|
||||
if (!ARMING_FLAG(ARMED)) {
|
||||
if (posControl.flags.hasValidPositionSensor) {
|
||||
if ((posControl.flags.estPosStatue >= EST_USABLE)) {
|
||||
setHomePosition(&posControl.actualState.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING );
|
||||
}
|
||||
}
|
||||
|
|
@ -1771,23 +1766,6 @@ int32_t getTotalTravelDistance(void)
|
|||
return lrintf(posControl.totalTripDistance);
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Set surface tracking target
|
||||
*-----------------------------------------------------------*/
|
||||
void setDesiredSurfaceOffset(float surfaceOffset)
|
||||
{
|
||||
if (surfaceOffset > 0) {
|
||||
posControl.desiredState.surface = constrainf(surfaceOffset, 1.0f, INAV_SURFACE_MAX_DISTANCE);
|
||||
}
|
||||
else {
|
||||
posControl.desiredState.surface = -1;
|
||||
}
|
||||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navTargetSurface = constrain(lrintf(posControl.desiredState.surface), -32678, 32767);
|
||||
#endif
|
||||
}
|
||||
|
||||
/*-----------------------------------------------------------
|
||||
* Calculate platform-specific hold position (account for deceleration)
|
||||
*-----------------------------------------------------------*/
|
||||
|
|
@ -2040,7 +2018,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
|
|||
wpLLH.alt = wpData->alt;
|
||||
|
||||
// WP #0 - special waypoint - HOME
|
||||
if ((wpNumber == 0) && ARMING_FLAG(ARMED) && posControl.flags.hasValidPositionSensor && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
|
||||
if ((wpNumber == 0) && ARMING_FLAG(ARMED) && (posControl.flags.estPosStatue >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled) {
|
||||
// Forcibly set home position. Note that this is only valid if already armed, otherwise home will be reset instantly
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
|
||||
setHomePosition(&wpPos.pos, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING);
|
||||
|
|
@ -2048,7 +2026,7 @@ void setWaypoint(uint8_t wpNumber, const navWaypoint_t * wpData)
|
|||
// WP #255 - special waypoint - directly set desiredPosition
|
||||
// Only valid when armed and in poshold mode
|
||||
else if ((wpNumber == 255) && (wpData->action == NAV_WP_ACTION_WAYPOINT) &&
|
||||
ARMING_FLAG(ARMED) && posControl.flags.hasValidPositionSensor && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
|
||||
ARMING_FLAG(ARMED) && (posControl.flags.estPosStatue >= EST_USABLE) && posControl.gpsOrigin.valid && posControl.flags.isGCSAssistedNavigationEnabled &&
|
||||
(posControl.navState == NAV_STATE_POSHOLD_2D_IN_PROGRESS || posControl.navState == NAV_STATE_POSHOLD_3D_IN_PROGRESS)) {
|
||||
// Convert to local coordinates
|
||||
geoConvertGeodeticToLocal(&posControl.gpsOrigin, &wpLLH, &wpPos.pos, GEO_ALT_RELATIVE);
|
||||
|
|
@ -2265,14 +2243,13 @@ void applyWaypointNavigationAndAltitudeHold(void)
|
|||
|
||||
#if defined(NAV_BLACKBOX)
|
||||
navFlags = 0;
|
||||
if (posControl.flags.hasValidAltitudeSensor) navFlags |= (1 << 0);
|
||||
if (posControl.flags.hasValidSurfaceSensor) navFlags |= (1 << 1);
|
||||
if (posControl.flags.hasValidPositionSensor) navFlags |= (1 << 2);
|
||||
//if (STATE(GPS_FIX)) navFlags |= (1 << 3);
|
||||
if (posControl.flags.estAltStatus == EST_TRUSTED) navFlags |= (1 << 0);
|
||||
if (posControl.flags.estSurfaceStatus == EST_TRUSTED) navFlags |= (1 << 1);
|
||||
if (posControl.flags.estPosStatue == EST_TRUSTED) navFlags |= (1 << 2);
|
||||
#if defined(NAV_GPS_GLITCH_DETECTION)
|
||||
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
|
||||
if (isGPSGlitchDetected()) navFlags |= (1 << 4);
|
||||
#endif
|
||||
if (posControl.flags.hasValidHeadingSensor) navFlags |= (1 << 5);
|
||||
if (posControl.flags.estHeadingStatus == EST_TRUSTED) navFlags |= (1 << 5);
|
||||
#endif
|
||||
|
||||
// Reset all navigation requests - NAV controllers will set them if necessary
|
||||
|
|
@ -2334,12 +2311,12 @@ void swithNavigationFlightModes(void)
|
|||
*-----------------------------------------------------------*/
|
||||
static bool canActivateAltHoldMode(void)
|
||||
{
|
||||
return posControl.flags.hasValidAltitudeSensor;
|
||||
return (posControl.flags.estAltStatus >= EST_USABLE);
|
||||
}
|
||||
|
||||
static bool canActivatePosHoldMode(void)
|
||||
{
|
||||
return posControl.flags.hasValidPositionSensor && posControl.flags.hasValidHeadingSensor;
|
||||
return (posControl.flags.estPosStatue >= EST_USABLE) && (posControl.flags.estHeadingStatus >= EST_USABLE);
|
||||
}
|
||||
|
||||
static navigationFSMEvent_t selectNavEventFromBoxModeInput(void)
|
||||
|
|
@ -2487,7 +2464,7 @@ bool navigationBlockArming(void)
|
|||
return false;
|
||||
|
||||
// Apply extra arming safety only if pilot has any of GPS modes configured
|
||||
if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !(posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME))) {
|
||||
if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatue >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
|
||||
shouldBlockArming = true;
|
||||
}
|
||||
|
||||
|
|
@ -2514,7 +2491,7 @@ bool navigationBlockArming(void)
|
|||
|
||||
bool navigationPositionEstimateIsHealthy(void)
|
||||
{
|
||||
return posControl.flags.hasValidPositionSensor && STATE(GPS_FIX_HOME);
|
||||
return (posControl.flags.estPosStatue >= EST_USABLE) && STATE(GPS_FIX_HOME);
|
||||
}
|
||||
|
||||
/**
|
||||
|
|
@ -2624,13 +2601,12 @@ void navigationInit(void)
|
|||
|
||||
posControl.flags.horizontalPositionDataNew = 0;
|
||||
posControl.flags.verticalPositionDataNew = 0;
|
||||
posControl.flags.surfaceDistanceDataNew = 0;
|
||||
posControl.flags.headingDataNew = 0;
|
||||
|
||||
posControl.flags.hasValidAltitudeSensor = 0;
|
||||
posControl.flags.hasValidPositionSensor = 0;
|
||||
posControl.flags.hasValidSurfaceSensor = 0;
|
||||
posControl.flags.hasValidHeadingSensor = 0;
|
||||
posControl.flags.estAltStatus = EST_NONE;
|
||||
posControl.flags.estPosStatue = EST_NONE;
|
||||
posControl.flags.estHeadingStatus = EST_NONE;
|
||||
posControl.flags.estSurfaceStatus = EST_NONE;
|
||||
|
||||
posControl.flags.forcedRTHActivated = 0;
|
||||
posControl.waypointCount = 0;
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue