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 ( )
{
// If we are on ground, or in constant position mode, or don't have the right vehicle and sensing to estimate wind, inhibit wind states
bool setWindInhibit = ( ! useAirspeed ( ) & & ! assume_zero_sideslip ( ) ) | | onGround | | ( PV_AidingMode = = AID_NONE ) ;
if ( ! inhibitWindStates & & setWindInhibit ) {
inhibitWindStates = true ;
2017-05-19 02:44:10 -03:00
updateStateIndexLim ( ) ;
2016-07-14 02:08:43 -03:00
} else if ( inhibitWindStates & & ! setWindInhibit ) {
inhibitWindStates = false ;
2017-05-19 02:44:10 -03:00
updateStateIndexLim ( ) ;
2016-07-14 02:08:43 -03:00
// set states and variances
if ( yawAlignComplete & & useAirspeed ( ) ) {
// if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading
// which assumes the vehicle has launched into the wind
Vector3f tempEuler ;
stateStruct . quat . to_euler ( tempEuler . x , tempEuler . y , tempEuler . z ) ;
float 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 ) ;
// set the wind sate variances to the measurement uncertainty
for ( uint8_t index = 22 ; index < = 23 ; index + + ) {
2020-11-07 02:24:13 -04:00
P [ index ] [ index ] = sq ( constrain_float ( frontend - > _easNoise , 0.5f , 5.0f ) * constrain_float ( dal . get_EAS2TAS ( ) , 0.9f , 10.0f ) ) ;
2016-07-14 02:08:43 -03:00
}
} else {
// set the variances using a typical wind speed
for ( uint8_t index = 22 ; index < = 23 ; index + + ) {
P [ index ] [ index ] = sq ( 5.0f ) ;
}
}
}
2019-02-22 19:35:24 -04:00
// determine if the vehicle is manoeuvring
2016-07-14 02:08:43 -03:00
if ( accNavMagHoriz > 0.5f ) {
manoeuvring = true ;
} else {
manoeuvring = false ;
}
// 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 ;
}
}
// 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 ;
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
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 ) ;
// Check if range beacon data is being used
bool rngBcnUsed = ( imuSampleTime_ms - lastRngBcnPassTime_ms < = minTestTime_ms ) ;
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
2020-05-12 02:13:48 -03:00
bool attAiding = posUsed | | gpsVelUsed | | optFlowUsed | | airSpdUsed | | rngBcnUsed | | bodyOdmUsed ;
2018-01-28 14:05:29 -04:00
// check if velocity drift has been constrained by a measurement source
bool velAiding = gpsVelUsed | | airSpdUsed | | optFlowUsed | | bodyOdmUsed ;
// 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 ) & &
( imuSampleTime_ms - lastRngBcnPassTime_ms > frontend - > tiltDriftTimeMax_ms ) & &
( 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 ;
}
posAidLossCritical = ( imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms ) & &
2020-04-16 08:04:53 -03:00
( imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms ) ;
2018-01-28 14:05:29 -04:00
}
2020-06-14 04:10:13 -03:00
// This is a special case for when we are a fixed wing aircraft vehicle that has landed and
// has no yaw measurement that works when static. Declare the yaw as unaligned (unknown)
// and declare attitude aiding loss so that we fall back into a non-aiding mode
2020-10-20 00:59:19 -03:00
if ( assume_zero_sideslip ( ) & & onGround & & ! use_compass ( ) & & ! using_external_yaw ( ) ) {
2020-06-14 04:10:13 -03:00
yawAlignComplete = false ;
finalInflightYawInit = false ;
attAidLossCritical = true ;
}
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 ;
rngBcnTimeout = true ;
tasTimeout = true ;
gpsNotAvailable = true ;
} 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 ;
2018-01-28 14:05:29 -04:00
rngBcnTimeout = true ;
gpsNotAvailable = true ;
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 ;
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 ) ;
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 ) ;
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m) " , ( unsigned ) imu_index , ( double ) receiverPos . x , ( double ) receiverPos . y ) ;
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF3 IMU%u initial beacon pos D offset = %3.1f (m) " , ( unsigned ) imu_index , ( double ) bcnPosOffsetNED . z ) ;
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 ;
posDownObsNoise = sq ( constrain_float ( extNavDataDelayed . posErr , 0.1f , 10.0f ) ) ;
ResetHeight ( ) ;
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 ;
lastRngBcnPassTime_ms = imuSampleTime_ms ;
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 ) {
2020-09-16 19:23:02 -03:00
if ( tiltErrorVariance < sq ( radians ( 5.0f ) ) ) {
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
{
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 ;
}
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 ;
}
return validOrigin & & tiltAlignComplete & & yawAlignComplete & & ( delAngBiasLearned | | assume_zero_sideslip ( ) ) & & gpsGoodToAlign & & gpsDataToFuse & & ! gpsInhibit ;
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
{
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 ;
}
2018-02-13 10:22:46 -04:00
return tiltAlignComplete & & yawAlignComplete & & delAngBiasLearned & & rngBcnAlignmentCompleted & & rngBcnDataToFuse ;
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
{
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 ;
}
2016-07-14 02:08:43 -03:00
// return true if we should use the compass
bool NavEKF3_core : : use_compass ( void ) const
{
2020-11-16 08:40:57 -04:00
const AP_NavEKF_Source : : SourceYaw yaw_source = frontend - > sources . getYawSource ( ) ;
2020-07-13 02:40:03 -03:00
if ( ( yaw_source ! = AP_NavEKF_Source : : SourceYaw : : COMPASS ) & &
( yaw_source ! = AP_NavEKF_Source : : SourceYaw : : EXTERNAL_COMPASS_FALLBACK ) ) {
// not using compass as a yaw source
return false ;
}
2020-11-18 02:19:59 -04:00
const auto * compass = dal . get_compass ( ) ;
2020-07-13 02:40:03 -03:00
return compass & &
2020-11-05 19:30:16 -04:00
compass - > use_for_yaw ( magSelectIndex ) & &
2020-04-10 21:43:46 -03:00
! allMagSensorsFailed ;
2016-07-14 02:08:43 -03:00
}
2020-04-10 21:43:46 -03:00
// are we using a yaw source other than the magnetomer?
2020-04-04 20:53:49 -03:00
bool NavEKF3_core : : using_external_yaw ( void ) const
{
2020-11-16 08:40:57 -04:00
const AP_NavEKF_Source : : SourceYaw yaw_source = frontend - > sources . getYawSource ( ) ;
2020-07-13 02:40:03 -03:00
if ( yaw_source = = AP_NavEKF_Source : : SourceYaw : : EXTERNAL | | yaw_source = = AP_NavEKF_Source : : SourceYaw : : EXTERNAL_COMPASS_FALLBACK | | ! use_compass ( ) ) {
2020-04-18 07:58:00 -03:00
return imuSampleTime_ms - last_gps_yaw_fusion_ms < 5000 | | imuSampleTime_ms - lastSynthYawTime_ms < 5000 ;
2020-04-04 20:53:49 -03:00
}
2020-04-10 21:43:46 -03:00
return false ;
2020-04-04 20:53:49 -03:00
}
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
}
// set the LLH location of the filters NED origin
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 ) ) {
2020-04-14 21:56:28 -03:00
// reject attempt to set origin if GPS is being used
2016-07-14 02:08:43 -03:00
return false ;
}
2020-06-17 08:28:09 -03:00
2016-07-14 02:08:43 -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 , loc . lat ) ;
2016-07-14 02:08:43 -03:00
validOrigin = true ;
return true ;
}
// Set the NED origin to be used until the next filter reset
2019-07-27 04:00:10 -03:00
void NavEKF3_core : : setOrigin ( const Location & loc )
2016-07-14 02:08:43 -03:00
{
2019-07-27 04:00:10 -03:00
EKF_origin = loc ;
2017-07-16 20:07:40 -03:00
// if flying, correct for height change from takeoff so that the origin is at field elevation
if ( inFlight ) {
EKF_origin . alt + = ( int32_t ) ( 100.0f * stateStruct . position . z ) ;
}
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
// put origin in frontend as well to ensure it stays in sync between lanes
frontend - > common_EKF_origin = EKF_origin ;
frontend - > common_origin_valid = 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
const float delAngBiasVarMax = sq ( radians ( 0.15f * dtEkfAvg ) ) ;
2020-11-16 08:40:57 -04:00
const AP_NavEKF_Source : : SourceYaw yaw_source = frontend - > sources . getYawSource ( ) ;
2020-07-13 02:40:03 -03:00
if ( ! use_compass ( ) & & ( yaw_source ! = AP_NavEKF_Source : : SourceYaw : : EXTERNAL ) & & ( yaw_source ! = AP_NavEKF_Source : : SourceYaw : : EXTERNAL_COMPASS_FALLBACK ) ) {
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
Vector3f delAngBiasVarVec = Vector3f ( P [ 10 ] [ 10 ] , P [ 11 ] [ 11 ] , P [ 12 ] [ 12 ] ) ;
Vector3f temp = prevTnb * delAngBiasVarVec ;
delAngBiasLearned = ( fabsf ( temp . x ) < delAngBiasVarMax ) & &
( fabsf ( temp . y ) < delAngBiasVarMax ) ;
} else {
delAngBiasLearned = ( P [ 10 ] [ 10 ] < = delAngBiasVarMax ) & &
( P [ 11 ] [ 11 ] < = delAngBiasVarMax ) & &
( P [ 12 ] [ 12 ] < = delAngBiasVarMax ) ;
}
2016-07-14 02:08:43 -03:00
}
// Commands the EKF to not use GPS.
2016-12-13 01:48:15 -04:00
// This command must be sent prior to vehicle arming and EKF commencement of GPS usage
2016-07-14 02:08:43 -03:00
// Returns 0 if command rejected
2016-12-13 01:48:15 -04:00
// Returns 1 if command accepted
2016-07-14 02:08:43 -03:00
uint8_t NavEKF3_core : : setInhibitGPS ( void )
{
2016-12-13 01:48:15 -04:00
if ( ( PV_AidingMode = = AID_ABSOLUTE ) | | motorsArmed ) {
2016-07-14 02:08:43 -03:00
return 0 ;
} else {
gpsInhibit = true ;
return 1 ;
}
}
// Update the filter status
void NavEKF3_core : : updateFilterStatus ( void )
{
// init return value
filterStatus . 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 ;
2016-07-14 02:08:43 -03:00
bool doingWindRelNav = ! tasTimeout & & assume_zero_sideslip ( ) ;
bool doingNormalGpsNav = ! posTimeout & & ( PV_AidingMode = = AID_ABSOLUTE ) ;
2020-05-18 02:22:20 -03:00
bool someVertRefData = ( ! velTimeout & & ( useGpsVertVel | | useExtNavVel ) ) | | ! hgtTimeout ;
2017-03-16 02:59:19 -03:00
bool someHorizRefData = ! ( velTimeout & & posTimeout & & tasTimeout ) | | 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
filterStatus . flags . attitude = ! stateStruct . quat . is_nan ( ) & & filterHealthy ; // attitude valid (we need a better check)
filterStatus . flags . horiz_vel = someHorizRefData & & filterHealthy ; // horizontal velocity estimate valid
filterStatus . flags . vert_vel = someVertRefData & & filterHealthy ; // vertical velocity estimate valid
2017-03-16 02:59:19 -03:00
filterStatus . flags . horiz_pos_rel = ( ( doingFlowNav & & gndOffsetValid ) | | doingWindRelNav | | doingNormalGpsNav | | doingBodyVelNav ) & & filterHealthy ; // relative horizontal position estimate valid
2016-07-14 02:08:43 -03:00
filterStatus . flags . horiz_pos_abs = doingNormalGpsNav & & filterHealthy ; // absolute horizontal position estimate valid
filterStatus . flags . vert_pos = ! hgtTimeout & & filterHealthy & & ! hgtNotAccurate ; // vertical position estimate valid
filterStatus . flags . terrain_alt = gndOffsetValid & & filterHealthy ; // terrain height estimate valid
filterStatus . flags . const_pos_mode = ( PV_AidingMode = = AID_NONE ) & & filterHealthy ; // constant position mode
2017-03-16 02:59:19 -03:00
filterStatus . flags . pred_horiz_pos_rel = filterStatus . flags . horiz_pos_rel ; // EKF3 enters the required mode before flight
filterStatus . flags . pred_horiz_pos_abs = filterStatus . flags . horiz_pos_abs ; // EKF3 enters the required mode before flight
2016-07-14 02:08:43 -03:00
filterStatus . flags . takeoff_detected = takeOffDetected ; // takeoff for optical flow navigation has been detected
2020-06-13 20:39:55 -03:00
filterStatus . flags . takeoff = expectTakeoff ; // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator
2016-07-14 02:08:43 -03:00
filterStatus . flags . touchdown = expectGndEffectTouchdown ; // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
filterStatus . flags . using_gps = ( ( imuSampleTime_ms - lastPosPassTime_ms ) < 4000 ) & & ( PV_AidingMode = = AID_ABSOLUTE ) ;
2020-11-16 08:40:57 -04:00
filterStatus . flags . gps_glitching = ! gpsAccuracyGood & & ( PV_AidingMode = = AID_ABSOLUTE ) & & ( frontend - > sources . getPosXYSource ( ) = = AP_NavEKF_Source : : SourceXY : : GPS ) ; // GPS glitching is affecting navigation accuracy
2018-07-13 21:10:20 -03:00
filterStatus . flags . gps_quality_good = gpsGoodToAlign ;
2020-02-07 16:33:52 -04:00
filterStatus . flags . initalized = filterStatus . flags . initalized | | healthy ( ) ;
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 ;
}
float trueAirspeed ;
if ( is_positive ( defaultAirSpeed ) & & assume_zero_sideslip ( ) ) {
if ( imuDataDelayed . time_ms - tasDataDelayed . time_ms < 5000 ) {
trueAirspeed = tasDataDelayed . tas ;
2020-03-10 03:48:08 -03:00
} else {
2020-06-17 08:28:09 -03:00
trueAirspeed = defaultAirSpeed * dal . get_EAS2TAS ( ) ;
2020-03-10 03:48:08 -03:00
}
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 ) {
Vector2f gpsVelNE = Vector2f ( gpsDataDelayed . vel . x , gpsDataDelayed . vel . y ) ;
float gpsVelAcc = fmaxf ( gpsSpdAccuracy , frontend - > _gpsHorizVelNoise ) ;
2020-04-24 18:52:52 -03:00
yawEstimator - > fuseVelData ( gpsVelNE , gpsVelAcc ) ;
2020-06-20 22:58:14 -03:00
// after velocity data has been fused the yaw variance esitmate will have been refreshed and
// is used maintain a history of validity
2020-11-15 20:53:35 -04:00
float yawEKFGSF , yawVarianceEKFGSF , velInnovLength ;
bool canUseEKFGSF = yawEstimator - > getYawData ( yawEKFGSF , yawVarianceEKFGSF ) & &
is_positive ( yawVarianceEKFGSF ) & &
yawVarianceEKFGSF < sq ( radians ( GSF_YAW_ACCURACY_THRESHOLD_DEG ) ) & &
( assume_zero_sideslip ( ) | | ( yawEstimator - > getVelInnovLength ( velInnovLength ) & & velInnovLength < frontend - > maxYawEstVelInnov ) ) ;
2020-06-20 22:58:14 -03:00
if ( canUseEKFGSF ) {
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 ) {
2020-03-10 03:48:08 -03:00
EKFGSF_resetMainFilterYaw ( ) ;
}
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 ;
}