2016-07-14 02:08:43 -03:00
# include <AP_HAL/AP_HAL.h>
# include "AP_NavEKF3.h"
# include "AP_NavEKF3_core.h"
# include <GCS_MAVLink/GCS.h>
2020-11-05 19:30:16 -04:00
# include "AP_DAL/AP_DAL.h"
2016-07-14 02:08:43 -03:00
// Control filter mode transitions
void NavEKF3_core : : controlFilterModes ( )
{
// Determine motor arm status
prevMotorsArmed = motorsArmed ;
2020-11-07 02:24:13 -04:00
motorsArmed = dal . get_armed ( ) ;
2016-07-14 02:08:43 -03:00
if ( motorsArmed & & ! prevMotorsArmed ) {
// set the time at which we arm to assist with checks
timeAtArming_ms = imuSampleTime_ms ;
}
// Detect if we are in flight on or ground
detectFlight ( ) ;
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
setWindMagStateLearningMode ( ) ;
// Check the alignmnent status of the tilt and yaw attitude
// Used during initial bootstrap alignment of the filter
checkAttitudeAlignmentStatus ( ) ;
// Set the type of inertial navigation aiding used
setAidingMode ( ) ;
}
/*
return effective value for _magCal for this core
*/
2020-01-20 00:52:12 -04:00
NavEKF3_core : : MagCal NavEKF3_core : : effective_magCal ( void ) const
2016-07-14 02:08:43 -03:00
{
2017-02-14 23:14:32 -04:00
// force use of simple magnetic heading fusion for specified cores
2020-07-13 02:40:03 -03:00
if ( frontend - > _magMask & core_index ) {
2020-01-20 00:52:12 -04:00
return MagCal : : NEVER ;
2017-02-14 23:14:32 -04:00
}
2020-01-22 05:56:57 -04:00
2020-11-18 02:19:38 -04:00
// handle deprecated MagCal::EXTERNAL_YAW and MagCal::EXTERNAL_YAW_FALLBACK values
2020-08-10 01:36:53 -03:00
const int8_t magCalParamVal = frontend - > _magCal . get ( ) ;
2020-11-19 21:57:12 -04:00
if ( magCalParamVal = = 5 ) {
2020-08-10 01:36:53 -03:00
return MagCal : : NEVER ;
}
2020-11-19 21:57:12 -04:00
if ( magCalParamVal = = 6 ) {
return MagCal : : WHEN_FLYING ;
}
2020-08-10 01:36:53 -03:00
return MagCal ( magCalParamVal ) ;
2016-07-14 02:08:43 -03:00
}
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
void NavEKF3_core : : setWindMagStateLearningMode ( )
{
2021-07-17 03:50:15 -03:00
const bool canEstimateWind = ( ( finalInflightYawInit & & dragFusionEnabled ) | | assume_zero_sideslip ( ) ) & &
! onGround & &
PV_AidingMode ! = AID_NONE ;
if ( ! inhibitWindStates & & ! canEstimateWind ) {
2016-07-14 02:08:43 -03:00
inhibitWindStates = true ;
2017-05-19 02:44:10 -03:00
updateStateIndexLim ( ) ;
2021-07-17 03:50:15 -03:00
} else if ( inhibitWindStates & & canEstimateWind & &
2021-07-17 23:44:49 -03:00
( sq ( stateStruct . velocity . x ) + sq ( stateStruct . velocity . y ) > sq ( 5.0f ) | | dragFusionEnabled ) ) {
2016-07-14 02:08:43 -03:00
inhibitWindStates = false ;
2017-05-19 02:44:10 -03:00
updateStateIndexLim ( ) ;
2016-07-14 02:08:43 -03:00
// set states and variances
2021-07-17 03:50:15 -03:00
if ( yawAlignComplete & & assume_zero_sideslip ( ) ) {
// if we have a valid heading, set the wind states to the reciprocal of the vehicle heading
2016-07-14 02:08:43 -03:00
// which assumes the vehicle has launched into the wind
2021-07-17 03:50:15 -03:00
// use airspeed if if recent data available
2021-05-04 08:12:23 -03:00
Vector3F tempEuler ;
2016-07-14 02:08:43 -03:00
stateStruct . quat . to_euler ( tempEuler . x , tempEuler . y , tempEuler . z ) ;
2021-07-17 03:50:15 -03:00
ftype trueAirspeedVariance ;
2022-07-03 19:01:54 -03:00
const bool haveAirspeedMeasurement = usingDefaultAirspeed | | ( tasDataDelayed . allowFusion & & ( imuDataDelayed . time_ms - tasDataDelayed . time_ms < 500 ) & & useAirspeed ( ) ) ;
2021-07-17 03:50:15 -03:00
if ( haveAirspeedMeasurement ) {
trueAirspeedVariance = constrain_ftype ( tasDataDelayed . tasVariance , WIND_VEL_VARIANCE_MIN , WIND_VEL_VARIANCE_MAX ) ;
const ftype windSpeed = sqrtF ( sq ( stateStruct . velocity . x ) + sq ( stateStruct . velocity . y ) ) - tasDataDelayed . tas ;
stateStruct . wind_vel . x = windSpeed * cosF ( tempEuler . z ) ;
stateStruct . wind_vel . y = windSpeed * sinF ( tempEuler . z ) ;
} else {
trueAirspeedVariance = sq ( WIND_VEL_VARIANCE_MAX ) ; // use 2-sigma for faster initial convergence
}
2016-07-14 02:08:43 -03:00
2020-11-20 17:39:04 -04:00
// set the wind state variances to the measurement uncertainty
2021-09-20 08:20:20 -03:00
zeroCols ( P , 22 , 23 ) ;
zeroRows ( P , 22 , 23 ) ;
P [ 22 ] [ 22 ] = P [ 23 ] [ 23 ] = trueAirspeedVariance ;
2021-07-17 03:50:15 -03:00
windStatesAligned = true ;
2016-07-14 02:08:43 -03:00
} else {
2021-07-17 03:50:15 -03:00
// set the variances using a typical max wind speed for small UAV operation
zeroCols ( P , 22 , 23 ) ;
zeroRows ( P , 22 , 23 ) ;
2016-07-14 02:08:43 -03:00
for ( uint8_t index = 22 ; index < = 23 ; index + + ) {
2021-07-17 03:50:15 -03:00
P [ index ] [ index ] = sq ( WIND_VEL_VARIANCE_MAX ) ;
2016-07-14 02:08:43 -03:00
}
}
}
2019-02-22 19:35:24 -04:00
// determine if the vehicle is manoeuvring
2021-02-03 10:04:39 -04:00
manoeuvring = accNavMagHoriz > 0.5f ;
2016-07-14 02:08:43 -03:00
// Determine if learning of magnetic field states has been requested by the user
bool magCalRequested =
2020-04-10 21:43:46 -03:00
( ( effectiveMagCal = = MagCal : : WHEN_FLYING ) & & inFlight ) | | // when flying
( ( effectiveMagCal = = MagCal : : WHEN_MANOEUVRING ) & & manoeuvring ) | | // when manoeuvring
( ( effectiveMagCal = = MagCal : : AFTER_FIRST_CLIMB ) & & finalInflightYawInit & & finalInflightMagInit ) | | // when initial in-air yaw and mag field reset is complete
( effectiveMagCal = = MagCal : : ALWAYS ) ; // all the time
2016-07-14 02:08:43 -03:00
// Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
// we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
2020-04-10 21:43:46 -03:00
bool magCalDenied = ! use_compass ( ) | | ( effectiveMagCal = = MagCal : : NEVER ) | | ( onGround & & effectiveMagCal ! = MagCal : : ALWAYS ) ;
2016-07-14 02:08:43 -03:00
// Inhibit the magnetic field calibration if not requested or denied
bool setMagInhibit = ! magCalRequested | | magCalDenied ;
if ( ! inhibitMagStates & & setMagInhibit ) {
inhibitMagStates = true ;
2017-05-19 02:44:10 -03:00
updateStateIndexLim ( ) ;
2020-09-16 23:32:22 -03:00
// variances will be reset in CovariancePrediction
2016-07-14 02:08:43 -03:00
} else if ( inhibitMagStates & & ! setMagInhibit ) {
inhibitMagStates = false ;
2017-05-19 02:44:10 -03:00
updateStateIndexLim ( ) ;
2016-07-14 02:08:43 -03:00
if ( magFieldLearned ) {
// if we have already learned the field states, then retain the learned variances
P [ 16 ] [ 16 ] = earthMagFieldVar . x ;
P [ 17 ] [ 17 ] = earthMagFieldVar . y ;
P [ 18 ] [ 18 ] = earthMagFieldVar . z ;
P [ 19 ] [ 19 ] = bodyMagFieldVar . x ;
P [ 20 ] [ 20 ] = bodyMagFieldVar . y ;
P [ 21 ] [ 21 ] = bodyMagFieldVar . z ;
} else {
// set the variances equal to the observation variances
2020-09-16 18:54:34 -03:00
for ( uint8_t index = 16 ; index < = 21 ; index + + ) {
2016-07-14 02:08:43 -03:00
P [ index ] [ index ] = sq ( frontend - > _magNoise ) ;
}
// set the NE earth magnetic field states using the published declination
// and set the corresponding variances and covariances
alignMagStateDeclination ( ) ;
}
// request a reset of the yaw and magnetic field states if not done before
if ( ! magStateInitComplete | | ( ! finalInflightMagInit & & inFlight ) ) {
magYawResetRequest = true ;
}
}
// inhibit delta velocity bias learning if we have not yet aligned the tilt
if ( tiltAlignComplete & & inhibitDelVelBiasStates ) {
// activate the states
inhibitDelVelBiasStates = false ;
2017-05-19 02:44:10 -03:00
updateStateIndexLim ( ) ;
2016-07-14 02:08:43 -03:00
// set the initial covariance values
2016-12-18 16:58:27 -04:00
P [ 13 ] [ 13 ] = sq ( ACCEL_BIAS_LIM_SCALER * frontend - > _accBiasLim * dtEkfAvg ) ;
2016-07-14 02:08:43 -03:00
P [ 14 ] [ 14 ] = P [ 13 ] [ 13 ] ;
P [ 15 ] [ 15 ] = P [ 13 ] [ 13 ] ;
}
if ( tiltAlignComplete & & inhibitDelAngBiasStates ) {
// activate the states
inhibitDelAngBiasStates = false ;
2017-05-19 02:44:10 -03:00
updateStateIndexLim ( ) ;
2016-07-14 02:08:43 -03:00
// set the initial covariance values
P [ 10 ] [ 10 ] = sq ( radians ( InitialGyroBiasUncertainty ( ) * dtEkfAvg ) ) ;
P [ 11 ] [ 11 ] = P [ 10 ] [ 10 ] ;
P [ 12 ] [ 12 ] = P [ 10 ] [ 10 ] ;
}
// If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
// because we want it re-done for each takeoff
if ( onGround ) {
finalInflightYawInit = false ;
finalInflightMagInit = false ;
2020-09-16 23:32:22 -03:00
magFieldLearned = false ;
2016-07-14 02:08:43 -03:00
}
2017-05-19 02:44:10 -03:00
updateStateIndexLim ( ) ;
}
// Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
// if we are not using those states
void NavEKF3_core : : updateStateIndexLim ( )
{
2017-05-09 19:07:21 -03:00
if ( inhibitWindStates ) {
if ( inhibitMagStates ) {
if ( inhibitDelVelBiasStates ) {
if ( inhibitDelAngBiasStates ) {
stateIndexLim = 9 ;
} else {
stateIndexLim = 12 ;
}
} else {
stateIndexLim = 15 ;
}
} else {
stateIndexLim = 21 ;
}
2016-07-14 02:08:43 -03:00
} else {
stateIndexLim = 23 ;
}
}
2024-04-16 21:01:21 -03:00
// set the default yaw source
void NavEKF3_core : : setYawSource ( )
{
AP_NavEKF_Source : : SourceYaw yaw_source = frontend - > sources . getYawSource ( ) ;
if ( wasLearningCompass_ms > 0 ) {
// can't use compass while it is being calibrated
if ( yaw_source = = AP_NavEKF_Source : : SourceYaw : : COMPASS ) {
yaw_source = AP_NavEKF_Source : : SourceYaw : : NONE ;
} else if ( yaw_source = = AP_NavEKF_Source : : SourceYaw : : GPS_COMPASS_FALLBACK ) {
yaw_source = AP_NavEKF_Source : : SourceYaw : : GPS ;
}
}
if ( yaw_source ! = yaw_source_last ) {
yaw_source_last = yaw_source ;
yaw_source_reset = true ;
}
}
2016-07-14 02:08:43 -03:00
// Set inertial navigation aiding mode
void NavEKF3_core : : setAidingMode ( )
{
2020-07-17 05:47:43 -03:00
resetDataSource posResetSource = resetDataSource : : DEFAULT ;
resetDataSource velResetSource = resetDataSource : : DEFAULT ;
2016-07-14 02:08:43 -03:00
// Save the previous status so we can detect when it has changed
PV_AidingModePrev = PV_AidingMode ;
2024-04-16 21:01:21 -03:00
setYawSource ( ) ;
2016-12-28 07:22:22 -04:00
// Check that the gyro bias variance has converged
checkGyroCalStatus ( ) ;
2016-12-16 21:22:07 -04:00
2020-12-11 16:22:44 -04:00
// Handle the special case where we are on ground and disarmed without a yaw measurement
// and navigating. This can occur if not using a magnetometer and yaw was aligned using GPS
// during the previous flight.
if ( yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : NONE & &
! motorsArmed & &
onGround & &
PV_AidingMode ! = AID_NONE )
{
PV_AidingMode = AID_NONE ;
yawAlignComplete = false ;
2023-04-15 21:14:31 -03:00
yawAlignGpsValidCount = 0 ;
2020-12-11 16:22:44 -04:00
finalInflightYawInit = false ;
2020-12-11 19:52:01 -04:00
ResetVelocity ( resetDataSource : : DEFAULT ) ;
ResetPosition ( resetDataSource : : DEFAULT ) ;
ResetHeight ( ) ;
2020-12-11 22:55:51 -04:00
// preserve quaternion 4x4 covariances, but zero the other rows and columns
for ( uint8_t row = 0 ; row < 4 ; row + + ) {
for ( uint8_t col = 4 ; col < 24 ; col + + ) {
P [ row ] [ col ] = 0.0f ;
}
}
for ( uint8_t col = 0 ; col < 4 ; col + + ) {
for ( uint8_t row = 4 ; row < 24 ; row + + ) {
P [ row ] [ col ] = 0.0f ;
}
}
// keep the IMU bias state variances, but zero the covariances
2021-05-04 08:12:23 -03:00
ftype oldBiasVariance [ 6 ] ;
2020-12-11 22:55:51 -04:00
for ( uint8_t row = 0 ; row < 6 ; row + + ) {
oldBiasVariance [ row ] = P [ row + 10 ] [ row + 10 ] ;
}
zeroCols ( P , 10 , 15 ) ;
zeroRows ( P , 10 , 15 ) ;
for ( uint8_t row = 0 ; row < 6 ; row + + ) {
P [ row + 10 ] [ row + 10 ] = oldBiasVariance [ row ] ;
}
2020-12-11 16:22:44 -04:00
}
2016-07-14 02:08:43 -03:00
// Determine if we should change aiding mode
2018-01-28 14:05:29 -04:00
switch ( PV_AidingMode ) {
case AID_NONE : {
// Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
// and IMU gyro bias estimates have stabilised
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
// GPS aiding is the preferred option unless excluded by the user
2020-04-14 21:56:28 -03:00
if ( readyToUseGPS ( ) | | readyToUseRangeBeacon ( ) | | readyToUseExtNav ( ) ) {
2018-01-28 14:05:29 -04:00
PV_AidingMode = AID_ABSOLUTE ;
2020-08-12 02:18:31 -03:00
} else if ( readyToUseOptFlow ( ) | | readyToUseBodyOdm ( ) ) {
2018-01-28 14:05:29 -04:00
PV_AidingMode = AID_RELATIVE ;
}
break ;
}
case AID_RELATIVE : {
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ( ( imuSampleTime_ms - prevFlowFuseTime_ms ) > 5000 ) ;
// Check if the fusion has timed out (body odometry measurements have been rejected for too long)
bool bodyOdmFusionTimeout = ( ( imuSampleTime_ms - prevBodyVelFuseTime_ms ) > 5000 ) ;
// Enable switch to absolute position mode if GPS or range beacon data is available
// If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
2020-08-21 22:40:30 -03:00
if ( readyToUseGPS ( ) | | readyToUseRangeBeacon ( ) | | readyToUseExtNav ( ) ) {
2018-01-28 14:05:29 -04:00
PV_AidingMode = AID_ABSOLUTE ;
} else if ( flowFusionTimeout & & bodyOdmFusionTimeout ) {
PV_AidingMode = AID_NONE ;
}
break ;
2016-07-14 02:08:43 -03:00
}
2018-01-28 14:05:29 -04:00
case AID_ABSOLUTE : {
// Find the minimum time without data required to trigger any check
uint16_t minTestTime_ms = MIN ( frontend - > tiltDriftTimeMax_ms , MIN ( frontend - > posRetryTimeNoVel_ms , frontend - > posRetryTimeUseVel_ms ) ) ;
// Check if optical flow data is being used
bool optFlowUsed = ( imuSampleTime_ms - prevFlowFuseTime_ms < = minTestTime_ms ) ;
// Check if body odometry data is being used
bool bodyOdmUsed = ( imuSampleTime_ms - prevBodyVelFuseTime_ms < = minTestTime_ms ) ;
// Check if airspeed data is being used
bool airSpdUsed = ( imuSampleTime_ms - lastTasPassTime_ms < = minTestTime_ms ) ;
2022-05-09 23:59:57 -03:00
// check if drag data is being used
bool dragUsed = ( imuSampleTime_ms - lastDragPassTime_ms < = minTestTime_ms ) ;
2022-01-27 20:09:44 -04:00
# if EK3_FEATURE_BEACON_FUSION
2018-01-28 14:05:29 -04:00
// Check if range beacon data is being used
2022-04-15 05:38:34 -03:00
const bool rngBcnUsed = ( imuSampleTime_ms - rngBcn . lastPassTime_ms < = minTestTime_ms ) ;
2022-01-27 20:09:44 -04:00
# else
const bool rngBcnUsed = false ;
# endif
2018-01-28 14:05:29 -04:00
2020-05-12 02:13:48 -03:00
// Check if GPS or external nav is being used
bool posUsed = ( imuSampleTime_ms - lastPosPassTime_ms < = minTestTime_ms ) ;
2018-01-28 14:05:29 -04:00
bool gpsVelUsed = ( imuSampleTime_ms - lastVelPassTime_ms < = minTestTime_ms ) ;
// Check if attitude drift has been constrained by a measurement source
2022-05-09 23:59:57 -03:00
bool attAiding = posUsed | | gpsVelUsed | | optFlowUsed | | airSpdUsed | | dragUsed | | rngBcnUsed | | bodyOdmUsed ;
2018-01-28 14:05:29 -04:00
// check if velocity drift has been constrained by a measurement source
2022-05-09 23:59:57 -03:00
bool velAiding = gpsVelUsed | | airSpdUsed | | dragUsed | | optFlowUsed | | bodyOdmUsed ;
2018-01-28 14:05:29 -04:00
// check if position drift has been constrained by a measurement source
2020-05-12 02:13:48 -03:00
bool posAiding = posUsed | | rngBcnUsed ;
2018-01-28 14:05:29 -04:00
// Check if the loss of attitude aiding has become critical
bool attAidLossCritical = false ;
if ( ! attAiding ) {
attAidLossCritical = ( imuSampleTime_ms - prevFlowFuseTime_ms > frontend - > tiltDriftTimeMax_ms ) & &
( imuSampleTime_ms - lastTasPassTime_ms > frontend - > tiltDriftTimeMax_ms ) & &
2022-01-27 20:09:44 -04:00
# if EK3_FEATURE_BEACON_FUSION
2022-04-15 05:38:34 -03:00
( imuSampleTime_ms - rngBcn . lastPassTime_ms > frontend - > tiltDriftTimeMax_ms ) & &
2022-01-27 20:09:44 -04:00
# endif
2018-01-28 14:05:29 -04:00
( imuSampleTime_ms - lastPosPassTime_ms > frontend - > tiltDriftTimeMax_ms ) & &
( imuSampleTime_ms - lastVelPassTime_ms > frontend - > tiltDriftTimeMax_ms ) ;
}
// Check if the loss of position accuracy has become critical
bool posAidLossCritical = false ;
2020-10-21 01:45:13 -03:00
if ( ! posAiding ) {
2018-01-28 14:05:29 -04:00
uint16_t maxLossTime_ms ;
if ( ! velAiding ) {
maxLossTime_ms = frontend - > posRetryTimeNoVel_ms ;
} else {
maxLossTime_ms = frontend - > posRetryTimeUseVel_ms ;
}
2022-01-27 20:09:44 -04:00
posAidLossCritical =
# if EK3_FEATURE_BEACON_FUSION
2022-04-15 05:38:34 -03:00
( imuSampleTime_ms - rngBcn . lastPassTime_ms > maxLossTime_ms ) & &
2022-01-27 20:09:44 -04:00
# endif
( imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms ) ;
2018-01-28 14:05:29 -04:00
}
if ( attAidLossCritical ) {
// if the loss of attitude data is critical, then put the filter into a constant position mode
PV_AidingMode = AID_NONE ;
posTimeout = true ;
velTimeout = true ;
tasTimeout = true ;
2022-05-09 23:59:57 -03:00
dragTimeout = true ;
2021-09-20 22:47:51 -03:00
gpsIsInUse = false ;
2018-01-28 14:05:29 -04:00
} else if ( posAidLossCritical ) {
// if the loss of position is critical, declare all sources of position aiding as being timed out
posTimeout = true ;
2020-10-25 22:49:30 -03:00
velTimeout = ! optFlowUsed & & ! gpsVelUsed & & ! bodyOdmUsed ;
2021-09-20 22:47:51 -03:00
gpsIsInUse = false ;
2020-03-10 03:48:08 -03:00
2018-01-28 14:05:29 -04:00
}
break ;
}
}
2016-07-14 02:08:43 -03:00
// check to see if we are starting or stopping aiding and set states and modes as required
if ( PV_AidingMode ! = PV_AidingModePrev ) {
2019-02-22 19:35:24 -04:00
// set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
2017-06-20 21:50:30 -03:00
switch ( PV_AidingMode ) {
case AID_NONE :
2016-07-14 02:08:43 -03:00
// We have ceased aiding
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_WARNING , " EKF3 IMU%u stopped aiding " , ( unsigned ) imu_index ) ;
2016-07-14 02:08:43 -03:00
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
posTimeout = true ;
velTimeout = true ;
// Reset the normalised innovation to avoid false failing bad fusion tests
velTestRatio = 0.0f ;
posTestRatio = 0.0f ;
2020-08-21 04:44:41 -03:00
// store the current position to be used to keep reporting the last known position
2016-07-14 02:08:43 -03:00
lastKnownPositionNE . x = stateStruct . position . x ;
lastKnownPositionNE . y = stateStruct . position . y ;
// initialise filtered altitude used to provide a takeoff reference to current baro on disarm
// this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
meaHgtAtTakeOff = baroDataDelayed . hgt ;
// reset the vertical position state to faster recover from baro errors experienced during touchdown
stateStruct . position . z = - meaHgtAtTakeOff ;
2022-04-07 21:52:08 -03:00
// store the current height to be used to keep reporting
// the last known position
lastKnownPositionD = stateStruct . position . z ;
2017-03-16 02:59:19 -03:00
// reset relative aiding sensor fusion activity status
flowFusionActive = false ;
bodyVelFusionActive = false ;
2017-06-20 21:50:30 -03:00
break ;
case AID_RELATIVE :
2017-03-16 02:59:19 -03:00
// We are doing relative position navigation where velocity errors are constrained, but position drift will occur
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u started relative aiding " , ( unsigned ) imu_index ) ;
2017-03-16 02:59:19 -03:00
if ( readyToUseOptFlow ( ) ) {
// Reset time stamps
flowValidMeaTime_ms = imuSampleTime_ms ;
prevFlowFuseTime_ms = imuSampleTime_ms ;
} else if ( readyToUseBodyOdm ( ) ) {
// Reset time stamps
lastbodyVelPassTime_ms = imuSampleTime_ms ;
prevBodyVelFuseTime_ms = imuSampleTime_ms ;
}
2016-07-14 02:08:43 -03:00
posTimeout = true ;
velTimeout = true ;
2017-06-20 21:50:30 -03:00
break ;
case AID_ABSOLUTE :
2016-12-28 07:22:22 -04:00
if ( readyToUseGPS ( ) ) {
2016-12-16 21:22:07 -04:00
// We are commencing aiding using GPS - this is the preferred method
2020-07-17 05:47:43 -03:00
posResetSource = resetDataSource : : GPS ;
velResetSource = resetDataSource : : GPS ;
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u is using GPS " , ( unsigned ) imu_index ) ;
2022-01-27 20:09:44 -04:00
# if EK3_FEATURE_BEACON_FUSION
2016-12-28 07:22:22 -04:00
} else if ( readyToUseRangeBeacon ( ) ) {
2016-12-16 21:22:07 -04:00
// We are commencing aiding using range beacons
2020-07-17 05:47:43 -03:00
posResetSource = resetDataSource : : RNGBCN ;
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u is using range beacons " , ( unsigned ) imu_index ) ;
2022-04-15 05:38:34 -03:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m) " , ( unsigned ) imu_index , ( double ) rngBcn . receiverPos . x , ( double ) rngBcn . receiverPos . y ) ;
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u initial beacon pos D offset = %3.1f (m) " , ( unsigned ) imu_index , ( double ) rngBcn . posOffsetNED . z ) ;
2022-01-27 20:09:44 -04:00
# endif // EK3_FEATURE_BEACON_FUSION
2021-01-19 00:27:03 -04:00
# if EK3_FEATURE_EXTERNAL_NAV
2020-04-14 21:56:28 -03:00
} else if ( readyToUseExtNav ( ) ) {
// we are commencing aiding using external nav
2020-07-17 05:47:43 -03:00
posResetSource = resetDataSource : : EXTNAV ;
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u is using external nav data " , ( unsigned ) imu_index ) ;
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u initial pos NED = %3.1f,%3.1f,%3.1f (m) " , ( unsigned ) imu_index , ( double ) extNavDataDelayed . pos . x , ( double ) extNavDataDelayed . pos . y , ( double ) extNavDataDelayed . pos . z ) ;
2020-07-16 07:03:29 -03:00
if ( useExtNavVel ) {
2020-07-17 05:47:43 -03:00
velResetSource = resetDataSource : : EXTNAV ;
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u initial vel NED = %3.1f,%3.1f,%3.1f (m/s) " , ( unsigned ) imu_index , ( double ) extNavVelDelayed . vel . x , ( double ) extNavVelDelayed . vel . y , ( double ) extNavVelDelayed . vel . z ) ;
2020-07-16 07:03:29 -03:00
}
2020-04-14 21:56:28 -03:00
// handle height reset as special case
hgtMea = - extNavDataDelayed . pos . z ;
2021-05-04 08:12:23 -03:00
posDownObsNoise = sq ( constrain_ftype ( extNavDataDelayed . posErr , 0.1f , 10.0f ) ) ;
2020-04-14 21:56:28 -03:00
ResetHeight ( ) ;
2021-01-19 00:27:03 -04:00
# endif // EK3_FEATURE_EXTERNAL_NAV
2016-07-14 02:08:43 -03:00
}
2016-12-16 21:22:07 -04:00
// clear timeout flags as a precaution to avoid triggering any additional transitions
posTimeout = false ;
velTimeout = false ;
2016-07-14 02:08:43 -03:00
// reset the last fusion accepted times to prevent unwanted activation of timeout logic
lastPosPassTime_ms = imuSampleTime_ms ;
lastVelPassTime_ms = imuSampleTime_ms ;
2022-01-27 20:09:44 -04:00
# if EK3_FEATURE_BEACON_FUSION
2022-04-15 05:38:34 -03:00
rngBcn . lastPassTime_ms = imuSampleTime_ms ;
2022-01-27 20:09:44 -04:00
# endif
2017-06-20 21:50:30 -03:00
break ;
2016-07-14 02:08:43 -03:00
}
// Always reset the position and velocity when changing mode
2020-07-17 05:47:43 -03:00
ResetVelocity ( velResetSource ) ;
ResetPosition ( posResetSource ) ;
2016-07-14 02:08:43 -03:00
}
}
// Check the tilt and yaw alignmnent status
// Used during initial bootstrap alignment of the filter
void NavEKF3_core : : checkAttitudeAlignmentStatus ( )
{
// Check for tilt convergence - used during initial alignment
2020-10-24 06:02:52 -03:00
// Once the tilt variances have reduced, re-set the yaw and magnetic field states
2017-04-04 04:05:28 -03:00
// and declare the tilt alignment complete
if ( ! tiltAlignComplete ) {
2021-05-04 08:12:23 -03:00
if ( tiltErrorVariance < sq ( radians ( 5.0 ) ) ) {
2017-04-04 04:05:28 -03:00
tiltAlignComplete = true ;
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u tilt alignment complete " , ( unsigned ) imu_index ) ;
2017-04-04 04:05:28 -03:00
}
2016-07-14 02:08:43 -03:00
}
2017-04-04 04:05:28 -03:00
// submit yaw and magnetic field reset request
if ( ! yawAlignComplete & & tiltAlignComplete & & use_compass ( ) ) {
2020-10-27 23:32:05 -03:00
magYawResetRequest = true ;
2016-07-14 02:08:43 -03:00
}
2017-07-19 21:40:11 -03:00
2016-07-14 02:08:43 -03:00
}
// return true if we should use the airspeed sensor
bool NavEKF3_core : : useAirspeed ( void ) const
{
2020-11-07 02:24:13 -04:00
return dal . airspeed_sensor_enabled ( ) ;
2016-07-14 02:08:43 -03:00
}
// return true if we should use the range finder sensor
bool NavEKF3_core : : useRngFinder ( void ) const
{
// TO-DO add code to set this based in setting of optical flow use parameter and presence of sensor
return true ;
}
2016-12-28 07:22:22 -04:00
// return true if the filter is ready to start using optical flow measurements
bool NavEKF3_core : : readyToUseOptFlow ( void ) const
2016-07-14 02:08:43 -03:00
{
2020-08-12 02:18:31 -03:00
// ensure flow is used for navigation and not terrain alt estimation
if ( frontend - > _flowUse ! = FLOW_USE_NAV ) {
return false ;
}
2020-11-16 08:40:57 -04:00
if ( ! frontend - > sources . useVelXYSource ( AP_NavEKF_Source : : SourceXY : : OPTFLOW ) ) {
2020-08-12 02:18:31 -03:00
return false ;
}
2016-12-28 07:22:22 -04:00
// We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use optical flow
return ( imuSampleTime_ms - flowMeaTime_ms < 200 ) & & tiltAlignComplete & & delAngBiasLearned ;
2016-07-14 02:08:43 -03:00
}
2017-03-16 02:59:19 -03:00
// return true if the filter is ready to start using body frame odometry measurements
bool NavEKF3_core : : readyToUseBodyOdm ( void ) const
{
2021-01-18 21:53:20 -04:00
# if EK3_FEATURE_BODY_ODOM
2020-11-16 08:40:57 -04:00
if ( ! frontend - > sources . useVelXYSource ( AP_NavEKF_Source : : SourceXY : : EXTNAV ) & &
! frontend - > sources . useVelXYSource ( AP_NavEKF_Source : : SourceXY : : WHEEL_ENCODER ) ) {
2020-11-18 02:19:38 -04:00
// exit immediately if sources not configured to fuse external nav or wheel encoders
2020-08-12 02:18:31 -03:00
return false ;
}
2017-07-27 02:01:48 -03:00
// Check for fresh visual odometry data that meets the accuracy required for alignment
bool visoDataGood = ( imuSampleTime_ms - bodyOdmMeasTime_ms < 200 ) & & ( bodyOdmDataNew . velErr < 1.0f ) ;
// Check for fresh wheel encoder data
2020-08-18 00:17:33 -03:00
bool wencDataGood = ( imuDataDelayed . time_ms - wheelOdmDataDelayed . time_ms < 200 ) ;
2017-07-27 02:01:48 -03:00
// We require stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use odometry measurements
2019-02-22 19:35:24 -04:00
// because they are in a body frame of reference
2017-07-27 02:01:48 -03:00
return ( visoDataGood | | wencDataGood )
2017-03-16 02:59:19 -03:00
& & tiltAlignComplete
& & delAngBiasLearned ;
2021-01-18 21:53:20 -04:00
# else
return false ;
# endif // EK3_FEATURE_BODY_ODOM
2017-03-16 02:59:19 -03:00
}
2016-07-14 02:08:43 -03:00
// return true if the filter to be ready to use gps
bool NavEKF3_core : : readyToUseGPS ( void ) const
{
2020-11-16 08:40:57 -04:00
if ( frontend - > sources . getPosXYSource ( ) ! = AP_NavEKF_Source : : SourceXY : : GPS ) {
2020-06-17 08:28:09 -03:00
return false ;
}
2020-11-20 15:38:49 -04:00
return validOrigin & & tiltAlignComplete & & yawAlignComplete & & ( delAngBiasLearned | | assume_zero_sideslip ( ) ) & & gpsGoodToAlign & & gpsDataToFuse ;
2016-07-14 02:08:43 -03:00
}
// return true if the filter to be ready to use the beacon range measurements
bool NavEKF3_core : : readyToUseRangeBeacon ( void ) const
{
2022-01-27 20:09:44 -04:00
# if EK3_FEATURE_BEACON_FUSION
2020-11-16 08:40:57 -04:00
if ( frontend - > sources . getPosXYSource ( ) ! = AP_NavEKF_Source : : SourceXY : : BEACON ) {
2020-08-12 02:18:31 -03:00
return false ;
}
2022-04-15 05:38:34 -03:00
return tiltAlignComplete & & yawAlignComplete & & delAngBiasLearned & & rngBcn . alignmentCompleted & & rngBcn . dataToFuse ;
2022-01-27 20:09:44 -04:00
# else
return false ;
# endif // EK3_FEATURE_BEACON_FUSION
2016-07-14 02:08:43 -03:00
}
2020-04-14 21:56:28 -03:00
// return true if the filter is ready to use external nav data
bool NavEKF3_core : : readyToUseExtNav ( void ) const
{
2021-01-19 00:27:03 -04:00
# if EK3_FEATURE_EXTERNAL_NAV
2020-11-16 08:40:57 -04:00
if ( frontend - > sources . getPosXYSource ( ) ! = AP_NavEKF_Source : : SourceXY : : EXTNAV ) {
2020-08-12 02:18:31 -03:00
return false ;
}
2020-04-14 21:56:28 -03:00
return tiltAlignComplete & & extNavDataToFuse ;
2021-01-19 00:27:03 -04:00
# else
return false ;
# endif // EK3_FEATURE_EXTERNAL_NAV
2020-04-14 21:56:28 -03:00
}
2016-07-14 02:08:43 -03:00
// return true if we should use the compass
bool NavEKF3_core : : use_compass ( void ) const
{
2024-04-16 21:01:21 -03:00
if ( ( yaw_source_last ! = AP_NavEKF_Source : : SourceYaw : : COMPASS ) & &
( yaw_source_last ! = AP_NavEKF_Source : : SourceYaw : : GPS_COMPASS_FALLBACK ) ) {
2020-07-13 02:40:03 -03:00
// not using compass as a yaw source
return false ;
}
2021-07-25 21:32:12 -03:00
const auto & compass = dal . compass ( ) ;
return compass . use_for_yaw ( magSelectIndex ) & &
2020-04-10 21:43:46 -03:00
! allMagSensorsFailed ;
2016-07-14 02:08:43 -03:00
}
2021-08-18 04:34:06 -03:00
// are we using (aka fusing) a non-compass yaw?
bool NavEKF3_core : : using_noncompass_for_yaw ( void ) const
2020-04-04 20:53:49 -03:00
{
2021-01-25 06:51:52 -04:00
# if EK3_FEATURE_EXTERNAL_NAV
2024-04-16 21:01:21 -03:00
if ( yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : EXTNAV ) {
2020-11-30 08:35:24 -04:00
return ( ( imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000 ) | | ( imuSampleTime_ms - lastSynthYawTime_ms < 5000 ) ) ;
}
2021-01-19 00:27:03 -04:00
# endif
2024-04-16 21:01:21 -03:00
if ( yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : GPS | | yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : GPS_COMPASS_FALLBACK | |
yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : GSF | | ! use_compass ( ) ) {
2021-05-03 08:52:18 -03:00
return imuSampleTime_ms - last_gps_yaw_ms < 5000 | | imuSampleTime_ms - lastSynthYawTime_ms < 5000 ;
2021-01-25 06:51:52 -04:00
}
2020-04-10 21:43:46 -03:00
return false ;
2020-04-04 20:53:49 -03:00
}
2021-08-18 04:44:33 -03:00
// are we using (aka fusing) external nav for yaw?
bool NavEKF3_core : : using_extnav_for_yaw ( ) const
{
# if EK3_FEATURE_EXTERNAL_NAV
2024-04-16 21:01:21 -03:00
if ( yaw_source_last = = AP_NavEKF_Source : : SourceYaw : : EXTNAV ) {
2021-08-18 04:44:33 -03:00
return ( ( imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000 ) | | ( imuSampleTime_ms - lastSynthYawTime_ms < 5000 ) ) ;
}
# endif
return false ;
}
2016-07-14 02:08:43 -03:00
/*
should we assume zero sideslip ?
*/
bool NavEKF3_core : : assume_zero_sideslip ( void ) const
{
// we don't assume zero sideslip for ground vehicles as EKF could
// be quite sensitive to a rapid spin of the ground vehicle if
// traction is lost
2020-11-07 02:24:13 -04:00
return dal . get_fly_forward ( ) & & dal . get_vehicle_class ( ) ! = AP_DAL : : VehicleClass : : GROUND ;
2016-07-14 02:08:43 -03:00
}
2021-06-16 15:15:53 -03:00
// sets the local NED origin using a LLH location (latitude, longitude, height)
// returns false if absolute aiding and GPS is being used or if the origin is already set
2017-04-19 03:29:17 -03:00
bool NavEKF3_core : : setOriginLLH ( const Location & loc )
2016-07-14 02:08:43 -03:00
{
2020-11-16 08:40:57 -04:00
if ( ( PV_AidingMode = = AID_ABSOLUTE ) & & ( frontend - > sources . getPosXYSource ( ) = = AP_NavEKF_Source : : SourceXY : : GPS ) ) {
2021-06-16 15:15:53 -03:00
// reject attempts to set the origin if GPS is being used or if the origin is already set
2016-07-14 02:08:43 -03:00
return false ;
}
2020-06-17 08:28:09 -03:00
2021-06-16 15:15:53 -03:00
return setOrigin ( loc ) ;
2016-07-14 02:08:43 -03:00
}
2021-06-16 15:15:53 -03:00
// sets the local NED origin using a LLH location (latitude, longitude, height)
// returns false is the origin has already been set
bool NavEKF3_core : : setOrigin ( const Location & loc )
2016-07-14 02:08:43 -03:00
{
2021-06-16 15:15:53 -03:00
// if the origin is valid reject setting a new origin
if ( validOrigin ) {
return false ;
}
2019-07-27 04:00:10 -03:00
EKF_origin = loc ;
2017-05-09 03:23:58 -03:00
ekfGpsRefHgt = ( double ) 0.01 * ( double ) EKF_origin . alt ;
2016-07-14 02:08:43 -03:00
// define Earth rotation vector in the NED navigation frame at the origin
2019-07-27 02:54:02 -03:00
calcEarthRateNED ( earthRateNED , EKF_origin . lat ) ;
2016-07-14 02:08:43 -03:00
validOrigin = true ;
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u origin set " , ( unsigned ) imu_index ) ;
2019-07-27 04:00:10 -03:00
2021-07-10 02:17:11 -03:00
if ( ! frontend - > common_origin_valid ) {
frontend - > common_origin_valid = true ;
// put origin in frontend as well to ensure it stays in sync between lanes
public_origin = EKF_origin ;
}
2021-06-16 15:15:53 -03:00
return true ;
2016-07-14 02:08:43 -03:00
}
// record a yaw reset event
void NavEKF3_core : : recordYawReset ( )
{
yawAlignComplete = true ;
if ( inFlight ) {
finalInflightYawInit = true ;
}
}
2016-12-28 07:22:22 -04:00
// set the class variable true if the delta angle bias variances are sufficiently small
void NavEKF3_core : : checkGyroCalStatus ( void )
2016-07-14 02:08:43 -03:00
{
// check delta angle bias variances
2021-05-04 08:12:23 -03:00
const ftype delAngBiasVarMax = sq ( radians ( 0.15 * dtEkfAvg ) ) ;
2024-04-16 21:01:21 -03:00
if ( ! use_compass ( ) & & ( yaw_source_last ! = AP_NavEKF_Source : : SourceYaw : : GPS ) & & ( yaw_source_last ! = AP_NavEKF_Source : : SourceYaw : : GPS_COMPASS_FALLBACK ) & &
( yaw_source_last ! = AP_NavEKF_Source : : SourceYaw : : EXTNAV ) ) {
2020-06-27 05:50:10 -03:00
// rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference
2020-03-10 03:48:08 -03:00
// which can make this check fail
2023-01-11 08:17:44 -04:00
const Vector3F delAngBiasVarVec { P [ 10 ] [ 10 ] , P [ 11 ] [ 11 ] , P [ 12 ] [ 12 ] } ;
const Vector3F temp = prevTnb * delAngBiasVarVec ;
2021-05-04 08:12:23 -03:00
delAngBiasLearned = ( fabsF ( temp . x ) < delAngBiasVarMax ) & &
( fabsF ( temp . y ) < delAngBiasVarMax ) ;
2020-03-10 03:48:08 -03:00
} else {
delAngBiasLearned = ( P [ 10 ] [ 10 ] < = delAngBiasVarMax ) & &
( P [ 11 ] [ 11 ] < = delAngBiasVarMax ) & &
( P [ 12 ] [ 12 ] < = delAngBiasVarMax ) ;
}
2016-07-14 02:08:43 -03:00
}
// Update the filter status
void NavEKF3_core : : updateFilterStatus ( void )
{
// init return value
2023-08-11 23:49:55 -03:00
nav_filter_status status ;
status . value = 0 ;
2017-03-16 02:59:19 -03:00
bool doingBodyVelNav = ( PV_AidingMode ! = AID_NONE ) & & ( imuSampleTime_ms - prevBodyVelFuseTime_ms < 5000 ) ;
bool doingFlowNav = ( PV_AidingMode ! = AID_NONE ) & & flowDataValid ;
2022-05-09 23:59:57 -03:00
bool doingWindRelNav = ( ! tasTimeout & & assume_zero_sideslip ( ) ) | | ! dragTimeout ;
2016-07-14 02:08:43 -03:00
bool doingNormalGpsNav = ! posTimeout & & ( PV_AidingMode = = AID_ABSOLUTE ) ;
2020-05-18 02:22:20 -03:00
bool someVertRefData = ( ! velTimeout & & ( useGpsVertVel | | useExtNavVel ) ) | | ! hgtTimeout ;
2022-05-09 23:59:57 -03:00
bool someHorizRefData = ! ( velTimeout & & posTimeout & & tasTimeout & & dragTimeout ) | | doingFlowNav | | doingBodyVelNav ;
2016-12-28 07:22:22 -04:00
bool filterHealthy = healthy ( ) & & tiltAlignComplete & & ( yawAlignComplete | | ( ! use_compass ( ) & & ( PV_AidingMode ! = AID_ABSOLUTE ) ) ) ;
2016-07-14 02:08:43 -03:00
// If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
2020-11-16 08:40:57 -04:00
bool hgtNotAccurate = ( frontend - > sources . getPosZSource ( ) = = AP_NavEKF_Source : : SourceZ : : GPS ) & & ! validOrigin ;
2016-07-14 02:08:43 -03:00
// set individual flags
2023-08-11 23:49:55 -03:00
status . flags . attitude = ! stateStruct . quat . is_nan ( ) & & filterHealthy ; // attitude valid (we need a better check)
status . flags . horiz_vel = someHorizRefData & & filterHealthy ; // horizontal velocity estimate valid
status . flags . vert_vel = someVertRefData & & filterHealthy ; // vertical velocity estimate valid
status . flags . horiz_pos_rel = ( ( doingFlowNav & & gndOffsetValid ) | | doingWindRelNav | | doingNormalGpsNav | | doingBodyVelNav ) & & filterHealthy ; // relative horizontal position estimate valid
status . flags . horiz_pos_abs = doingNormalGpsNav & & filterHealthy ; // absolute horizontal position estimate valid
status . flags . vert_pos = ! hgtTimeout & & filterHealthy & & ! hgtNotAccurate ; // vertical position estimate valid
status . flags . terrain_alt = gndOffsetValid & & filterHealthy ; // terrain height estimate valid
status . flags . const_pos_mode = ( PV_AidingMode = = AID_NONE ) & & filterHealthy ; // constant position mode
status . flags . pred_horiz_pos_rel = status . flags . horiz_pos_rel ; // EKF3 enters the required mode before flight
status . flags . pred_horiz_pos_abs = status . flags . horiz_pos_abs ; // EKF3 enters the required mode before flight
status . flags . takeoff_detected = takeOffDetected ; // takeoff for optical flow navigation has been detected
status . flags . takeoff = dal . get_takeoff_expected ( ) ; // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator
status . flags . touchdown = dal . get_touchdown_expected ( ) ; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
status . flags . using_gps = ( ( imuSampleTime_ms - lastPosPassTime_ms ) < 4000 ) & & ( PV_AidingMode = = AID_ABSOLUTE ) ;
status . flags . gps_glitching = ! gpsAccuracyGood & & ( PV_AidingMode = = AID_ABSOLUTE ) & & ( frontend - > sources . getPosXYSource ( ) = = AP_NavEKF_Source : : SourceXY : : GPS ) ; // GPS glitching is affecting navigation accuracy
status . flags . gps_quality_good = gpsGoodToAlign ;
2022-01-26 02:35:14 -04:00
// for reporting purposes we report rejecting airspeed after 3s of not fusing when we want to fuse the data
2023-08-11 23:49:55 -03:00
status . flags . rejecting_airspeed = lastTasFailTime_ms ! = 0 & &
2022-01-26 02:35:14 -04:00
( imuSampleTime_ms - lastTasFailTime_ms ) < 1000 & &
( imuSampleTime_ms - lastTasPassTime_ms ) > 3000 ;
2023-08-11 23:49:55 -03:00
status . flags . initalized = status . flags . initalized | | healthy ( ) ;
status . flags . dead_reckoning = ( PV_AidingMode ! = AID_NONE ) & & doingWindRelNav & & ! ( ( doingFlowNav & & gndOffsetValid ) | | doingNormalGpsNav | | doingBodyVelNav ) ;
filterStatus . value = status . value ;
2016-07-14 02:08:43 -03:00
}
2020-04-23 03:31:12 -03:00
void NavEKF3_core : : runYawEstimatorPrediction ( )
2020-03-10 03:48:08 -03:00
{
2020-06-17 08:28:09 -03:00
// exit immediately if no yaw estimator
if ( yawEstimator = = nullptr ) {
return ;
}
// ensure GPS is used for horizontal position and velocity
2020-11-16 08:40:57 -04:00
if ( frontend - > sources . getPosXYSource ( ) ! = AP_NavEKF_Source : : SourceXY : : GPS | |
! frontend - > sources . useVelXYSource ( AP_NavEKF_Source : : SourceXY : : GPS ) ) {
2020-06-17 08:28:09 -03:00
return ;
}
2021-05-04 08:12:23 -03:00
ftype trueAirspeed ;
2022-07-03 19:01:54 -03:00
if ( tasDataDelayed . allowFusion & & assume_zero_sideslip ( ) ) {
2021-01-05 20:42:34 -04:00
trueAirspeed = MAX ( tasDataDelayed . tas , 0.0f ) ;
2020-06-17 08:28:09 -03:00
} else {
trueAirspeed = 0.0f ;
2020-04-23 03:31:12 -03:00
}
2020-06-17 08:28:09 -03:00
yawEstimator - > update ( imuDataDelayed . delAng , imuDataDelayed . delVel , imuDataDelayed . delAngDT , imuDataDelayed . delVelDT , EKFGSF_run_filterbank , trueAirspeed ) ;
2020-04-23 03:31:12 -03:00
}
2020-03-10 03:48:08 -03:00
2020-04-23 03:31:12 -03:00
void NavEKF3_core : : runYawEstimatorCorrection ( )
{
2020-06-17 08:28:09 -03:00
// exit immediately if no yaw estimator
if ( yawEstimator = = nullptr ) {
return ;
}
// ensure GPS is used for horizontal position and velocity
2020-11-16 08:40:57 -04:00
if ( frontend - > sources . getPosXYSource ( ) ! = AP_NavEKF_Source : : SourceXY : : GPS | |
! frontend - > sources . useVelXYSource ( AP_NavEKF_Source : : SourceXY : : GPS ) ) {
2020-06-17 08:28:09 -03:00
return ;
}
if ( EKFGSF_run_filterbank ) {
2020-03-10 03:48:08 -03:00
if ( gpsDataToFuse ) {
2021-05-04 08:12:23 -03:00
Vector2F gpsVelNE = Vector2F ( gpsDataDelayed . vel . x , gpsDataDelayed . vel . y ) ;
ftype gpsVelAcc = fmaxF ( gpsSpdAccuracy , ftype ( frontend - > _gpsHorizVelNoise ) ) ;
2020-04-24 18:52:52 -03:00
yawEstimator - > fuseVelData ( gpsVelNE , gpsVelAcc ) ;
2020-06-20 22:58:14 -03:00
2020-12-15 03:20:31 -04:00
// after velocity data has been fused the yaw variance estimate will have been refreshed and
2020-06-20 22:58:14 -03:00
// is used maintain a history of validity
2021-05-04 08:12:23 -03:00
ftype gsfYaw , gsfYawVariance ;
2020-12-15 03:20:31 -04:00
if ( EKFGSF_getYaw ( gsfYaw , gsfYawVariance ) ) {
2020-06-20 22:58:14 -03:00
if ( EKFGSF_yaw_valid_count < GSF_YAW_VALID_HISTORY_THRESHOLD ) {
EKFGSF_yaw_valid_count + + ;
}
} else {
EKFGSF_yaw_valid_count = 0 ;
}
2020-03-10 03:48:08 -03:00
}
// action an external reset request
2020-04-20 21:33:06 -03:00
if ( EKFGSF_yaw_reset_request_ms > 0 & & imuSampleTime_ms - EKFGSF_yaw_reset_request_ms < YAW_RESET_TO_GSF_TIMEOUT_MS ) {
2021-08-05 00:24:33 -03:00
EKFGSF_resetMainFilterYaw ( true ) ;
2020-03-10 03:48:08 -03:00
}
2020-11-20 20:39:07 -04:00
} else {
EKFGSF_yaw_valid_count = 0 ;
2020-03-10 03:48:08 -03:00
}
}
// request a reset the yaw to the GSF estimate
// request times out after YAW_RESET_TO_GSF_TIMEOUT_MS if it cannot be actioned
void NavEKF3_core : : EKFGSF_requestYawReset ( )
{
EKFGSF_yaw_reset_request_ms = imuSampleTime_ms ;
}