2015-10-05 23:30:07 -03:00
# include <AP_HAL/AP_HAL.h>
# include "AP_NavEKF2.h"
# include "AP_NavEKF2_core.h"
# include <AP_AHRS/AP_AHRS.h>
2016-07-18 23:16:25 -03:00
# include <GCS_MAVLink/GCS.h>
2015-10-05 23:30:07 -03:00
# include <stdio.h>
extern const AP_HAL : : HAL & hal ;
// Control filter mode transitions
void NavEKF2_core : : controlFilterModes ( )
{
// Determine motor arm status
prevMotorsArmed = motorsArmed ;
motorsArmed = hal . util - > get_soft_armed ( ) ;
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 ( ) ;
}
2016-05-27 08:22:19 -03:00
/*
return effective value for _magCal for this core
*/
uint8_t NavEKF2_core : : effective_magCal ( void ) const
{
2017-02-14 23:15:01 -04:00
// force use of simple magnetic heading fusion for specified cores
if ( frontend - > _magMask & core_index ) {
return 2 ;
} else {
return frontend - > _magCal ;
}
2016-05-27 08:22:19 -03:00
}
2015-10-05 23:30:07 -03:00
// Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
// avoid unnecessary operations
void NavEKF2_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
2016-05-23 22:49:42 -03:00
bool setWindInhibit = ( ! useAirspeed ( ) & & ! assume_zero_sideslip ( ) ) | | onGround | | ( PV_AidingMode = = AID_NONE ) ;
if ( ! inhibitWindStates & & setWindInhibit ) {
inhibitWindStates = true ;
} else if ( inhibitWindStates & & ! setWindInhibit ) {
inhibitWindStates = false ;
// 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 + + ) {
P [ index ] [ index ] = sq ( constrain_float ( frontend - > _easNoise , 0.5f , 5.0f ) * constrain_float ( _ahrs - > get_EAS2TAS ( ) , 0.9f , 10.0f ) ) ;
}
} else {
// set the variances using a typical wind speed
for ( uint8_t index = 22 ; index < = 23 ; index + + ) {
P [ index ] [ index ] = sq ( 5.0f ) ;
}
}
}
2015-10-05 23:30:07 -03:00
2019-02-21 13:33:38 -04:00
// determine if the vehicle is manoeuvring
2015-10-05 23:30:07 -03:00
if ( accNavMagHoriz > 0.5f ) {
manoeuvring = true ;
} else {
manoeuvring = false ;
}
// Determine if learning of magnetic field states has been requested by the user
2016-05-27 08:22:19 -03:00
uint8_t magCal = effective_magCal ( ) ;
2015-10-16 09:18:23 -03:00
bool magCalRequested =
2016-05-27 08:22:19 -03:00
( ( magCal = = 0 ) & & inFlight ) | | // when flying
( ( magCal = = 1 ) & & manoeuvring ) | | // when manoeuvring
2016-06-09 20:19:28 -03:00
( ( magCal = = 3 ) & & finalInflightYawInit & & finalInflightMagInit ) | | // when initial in-air yaw and mag field reset is complete
2016-05-27 08:22:19 -03:00
( magCal = = 4 ) ; // all the time
2015-10-16 09:18:23 -03:00
2015-10-28 06:06:40 -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)
2016-05-31 03:02:52 -03:00
bool magCalDenied = ! use_compass ( ) | | ( magCal = = 2 ) | | ( onGround & & magCal ! = 4 ) ;
2015-10-05 23:30:07 -03:00
// Inhibit the magnetic field calibration if not requested or denied
2016-05-23 21:23:49 -03:00
bool setMagInhibit = ! magCalRequested | | magCalDenied ;
if ( ! inhibitMagStates & & setMagInhibit ) {
inhibitMagStates = true ;
2020-09-16 21:39:04 -03:00
// variances will be reset in CovariancePrediction
2016-05-23 21:23:49 -03:00
} else if ( inhibitMagStates & & ! setMagInhibit ) {
inhibitMagStates = false ;
2016-07-02 05:46:42 -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-02 05:46:42 -03:00
P [ index ] [ index ] = sq ( frontend - > _magNoise ) ;
}
2016-07-04 10:54:28 -03:00
// set the NE earth magnetic field states using the published declination
// and set the corresponding variances and covariances
alignMagStateDeclination ( ) ;
2016-05-23 21:23:49 -03:00
}
2016-05-31 03:02:52 -03:00
// request a reset of the yaw and magnetic field states if not done before
2016-06-09 20:19:28 -03:00
if ( ! magStateInitComplete | | ( ! finalInflightMagInit & & inFlight ) ) {
2016-05-31 03:02:52 -03:00
magYawResetRequest = true ;
}
2016-05-23 21:23:49 -03:00
}
2015-10-05 23:30:07 -03:00
2015-10-30 08:05:31 -03:00
// 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 ) {
2016-06-09 20:19:28 -03:00
finalInflightYawInit = false ;
finalInflightMagInit = false ;
2015-10-28 06:06:40 -03:00
}
2015-10-05 23:30:07 -03:00
// 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
if ( inhibitMagStates & & inhibitWindStates ) {
stateIndexLim = 15 ;
} else if ( inhibitWindStates ) {
stateIndexLim = 21 ;
} else {
stateIndexLim = 23 ;
}
}
// Set inertial navigation aiding mode
void NavEKF2_core : : setAidingMode ( )
{
// Save the previous status so we can detect when it has changed
2016-07-15 00:09:48 -03:00
PV_AidingModePrev = PV_AidingMode ;
2016-07-15 09:54:06 -03:00
// Determine if we should change aiding mode
2016-12-22 20:31:33 -04:00
switch ( PV_AidingMode ) {
case AID_NONE : {
2016-07-04 20:12:59 -03:00
// 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
2016-07-06 02:33:29 -03:00
bool filterIsStable = tiltAlignComplete & & yawAlignComplete & & checkGyroCalStatus ( ) ;
2016-07-04 20:12:59 -03:00
// If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
2016-10-25 17:54:29 -03:00
// GPS aiding is the preferred option unless excluded by the user
bool canUseGPS = ( ( frontend - > _fusionModeGPS ) ! = 3 & & readyToUseGPS ( ) & & filterIsStable & & ! gpsInhibit ) ;
bool canUseRangeBeacon = readyToUseRangeBeacon ( ) & & filterIsStable ;
2018-03-07 00:42:31 -04:00
bool canUseExtNav = readyToUseExtNav ( ) ;
if ( canUseGPS | | canUseRangeBeacon | | canUseExtNav ) {
2016-07-15 00:09:48 -03:00
PV_AidingMode = AID_ABSOLUTE ;
2019-03-14 15:45:28 -03:00
} else if ( optFlowDataPresent ( ) & & ( frontend - > _flowUse = = FLOW_USE_NAV ) & & filterIsStable ) {
2016-07-15 00:09:48 -03:00
PV_AidingMode = AID_RELATIVE ;
}
2016-12-22 20:31:33 -04:00
}
break ;
case AID_RELATIVE : {
// Check if the optical flow sensor has timed out
bool flowSensorTimeout = ( ( imuSampleTime_ms - flowValidMeaTime_ms ) > 5000 ) ;
// Check if the fusion has timed out (flow measurements have been rejected for too long)
bool flowFusionTimeout = ( ( imuSampleTime_ms - prevFlowFuseTime_ms ) > 5000 ) ;
// Enable switch to absolute position mode if GPS is available
// If GPS is not available and flow fusion has timed out, then fall-back to no-aiding
if ( ( frontend - > _fusionModeGPS ) ! = 3 & & readyToUseGPS ( ) & & ! gpsInhibit ) {
PV_AidingMode = AID_ABSOLUTE ;
} else if ( flowSensorTimeout | | flowFusionTimeout ) {
PV_AidingMode = AID_NONE ;
}
}
break ;
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 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 ) ;
// Check if GPS is being used
2018-03-07 00:42:31 -04:00
bool posUsed = ( imuSampleTime_ms - lastPosPassTime_ms < = minTestTime_ms ) ;
2016-12-22 20:31:33 -04:00
bool gpsVelUsed = ( imuSampleTime_ms - lastVelPassTime_ms < = minTestTime_ms ) ;
2020-04-17 22:52:51 -03:00
// Check if external nav is being used
bool extNavUsed = ( imuSampleTime_ms - lastExtNavPassTime_ms < = minTestTime_ms ) ;
2016-12-22 20:31:33 -04:00
// Check if attitude drift has been constrained by a measurement source
2020-04-17 22:52:51 -03:00
bool attAiding = posUsed | | gpsVelUsed | | optFlowUsed | | airSpdUsed | | rngBcnUsed | | extNavUsed ;
2016-12-22 20:31:33 -04:00
// check if velocity drift has been constrained by a measurement source
bool velAiding = gpsVelUsed | | airSpdUsed | | optFlowUsed ;
// check if position drift has been constrained by a measurement source
2020-04-17 22:52:51 -03:00
bool posAiding = posUsed | | rngBcnUsed | | extNavUsed ;
2016-12-22 20:31:33 -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 ) & &
2020-04-17 22:52:51 -03:00
( imuSampleTime_ms - lastExtNavPassTime_ms > frontend - > tiltDriftTimeMax_ms ) & &
2016-12-22 20:31:33 -04:00
( imuSampleTime_ms - lastVelPassTime_ms > frontend - > tiltDriftTimeMax_ms ) ;
}
// Check if the loss of position accuracy has become critical
bool posAidLossCritical = false ;
if ( ! posAiding ) {
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-17 22:52:51 -03:00
( imuSampleTime_ms - lastExtNavPassTime_ms > maxLossTime_ms ) & &
( imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms ) ;
2016-12-22 20:31:33 -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 ) {
2019-10-02 22:52:40 -03:00
if ( ( frontend - > _flowUse = = FLOW_USE_NAV ) & & optFlowDataPresent ( ) & & ( imuSampleTime_ms - rngValidMeaTime_ms < 500 ) ) {
PV_AidingMode = AID_NONE ;
}
2016-12-22 20:31:33 -04:00
// if the loss of position is critical, declare all sources of position aiding as being timed out
posTimeout = true ;
velTimeout = true ;
rngBcnTimeout = true ;
gpsNotAvailable = true ;
}
break ;
}
2018-10-02 20:39:05 -03:00
}
2016-07-15 00:09:48 -03:00
2015-10-05 23:30:07 -03:00
// check to see if we are starting or stopping aiding and set states and modes as required
2016-07-15 00:09:48 -03:00
if ( PV_AidingMode ! = PV_AidingModePrev ) {
2019-02-21 13:33:38 -04:00
// set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
2016-12-22 20:31:33 -04:00
switch ( PV_AidingMode ) {
case AID_NONE :
2015-10-05 23:30:07 -03:00
// We have ceased aiding
2017-07-09 01:16:08 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_WARNING , " EKF2 IMU%u has stopped aiding " , ( unsigned ) imu_index ) ;
2015-10-05 23:30:07 -03:00
// When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
posTimeout = true ;
2016-07-15 00:09:48 -03:00
velTimeout = true ;
// Reset the normalised innovation to avoid false failing bad fusion tests
velTestRatio = 0.0f ;
posTestRatio = 0.0f ;
2015-10-05 23:30:07 -03:00
// store the current position to be used to keep reporting the last known position
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 ;
2016-12-22 20:31:33 -04:00
break ;
case AID_RELATIVE :
2016-05-12 14:04:56 -03:00
// We have commenced aiding, but GPS usage has been prohibited so use optical flow only
2017-07-09 01:16:08 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " EKF2 IMU%u is using optical flow " , ( unsigned ) imu_index ) ;
2015-10-05 23:30:07 -03:00
posTimeout = true ;
velTimeout = true ;
// Reset the last valid flow measurement time
flowValidMeaTime_ms = imuSampleTime_ms ;
// Reset the last valid flow fusion time
prevFlowFuseTime_ms = imuSampleTime_ms ;
2016-12-22 20:31:33 -04:00
break ;
case AID_ABSOLUTE : {
2016-10-25 17:54:29 -03:00
bool canUseGPS = ( ( frontend - > _fusionModeGPS ) ! = 3 & & readyToUseGPS ( ) & & ! gpsInhibit ) ;
bool canUseRangeBeacon = readyToUseRangeBeacon ( ) ;
2018-03-07 00:42:31 -04:00
bool canUseExtNav = readyToUseExtNav ( ) ;
2016-05-12 14:04:56 -03:00
// We have commenced aiding and GPS usage is allowed
2016-10-25 17:54:29 -03:00
if ( canUseGPS ) {
2017-07-09 01:16:08 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " EKF2 IMU%u is using GPS " , ( unsigned ) imu_index ) ;
2016-10-25 17:54:29 -03:00
}
2015-10-05 23:30:07 -03:00
posTimeout = false ;
velTimeout = false ;
2016-10-25 17:54:29 -03:00
// We have commenced aiding and range beacon usage is allowed
if ( canUseRangeBeacon ) {
2017-07-09 01:16:08 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " EKF2 IMU%u is using range beacons " , ( unsigned ) imu_index ) ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " EKF2 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 , " EKF2 IMU%u initial beacon pos D offset = %3.1f (m) " , ( unsigned ) imu_index , ( double ) bcnPosOffset ) ;
2016-10-25 17:54:29 -03:00
}
2018-03-07 00:42:31 -04:00
// We have commenced aiding and external nav usage is allowed
if ( canUseExtNav ) {
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " EKF2 IMU%u is using external nav data " , ( unsigned ) imu_index ) ;
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " EKF2 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 ) ;
2019-08-14 03:35:29 -03:00
//handle yaw reset as special case only if compass is disabled
if ( ! use_compass ( ) ) {
extNavYawResetRequest = true ;
controlMagYawReset ( ) ;
}
2018-03-07 00:42:31 -04:00
// handle height reset as special case
hgtMea = - extNavDataDelayed . pos . z ;
posDownObsNoise = sq ( constrain_float ( extNavDataDelayed . posErr , 0.1f , 10.0f ) ) ;
ResetHeight ( ) ;
}
2016-10-25 17:54:29 -03:00
// reset the last fusion accepted times to prevent unwanted activation of timeout logic
2015-10-05 23:30:07 -03:00
lastPosPassTime_ms = imuSampleTime_ms ;
2016-10-25 17:54:29 -03:00
lastVelPassTime_ms = imuSampleTime_ms ;
lastRngBcnPassTime_ms = imuSampleTime_ms ;
2020-04-17 22:52:51 -03:00
lastExtNavPassTime_ms = imuSampleTime_ms ;
2016-12-22 20:31:33 -04:00
}
break ;
2015-10-05 23:30:07 -03:00
}
2016-07-15 00:09:48 -03:00
// Always reset the position and velocity when changing mode
2015-10-05 23:30:07 -03:00
ResetVelocity ( ) ;
ResetPosition ( ) ;
}
}
2016-05-31 03:02:52 -03:00
// Check the tilt and yaw alignmnent status
2015-10-05 23:30:07 -03:00
// Used during initial bootstrap alignment of the filter
void NavEKF2_core : : checkAttitudeAlignmentStatus ( )
{
// Check for tilt convergence - used during initial alignment
2015-11-09 20:25:44 -04:00
float alpha = 1.0f * imuDataDelayed . delAngDT ;
2015-10-05 23:30:07 -03:00
float temp = tiltErrVec . length ( ) ;
tiltErrFilt = alpha * temp + ( 1.0f - alpha ) * tiltErrFilt ;
if ( tiltErrFilt < 0.005f & & ! tiltAlignComplete ) {
tiltAlignComplete = true ;
2017-07-09 01:16:08 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " EKF2 IMU%u tilt alignment complete " , ( unsigned ) imu_index ) ;
2015-10-05 23:30:07 -03:00
}
2016-05-31 03:02:52 -03:00
// submit yaw and magnetic field reset requests depending on whether we have compass data
if ( tiltAlignComplete & & ! yawAlignComplete ) {
if ( use_compass ( ) ) {
magYawResetRequest = true ;
gpsYawResetRequest = false ;
} else {
magYawResetRequest = false ;
gpsYawResetRequest = true ;
}
2015-10-05 23:30:07 -03:00
}
}
// return true if we should use the airspeed sensor
bool NavEKF2_core : : useAirspeed ( void ) const
{
return _ahrs - > airspeed_sensor_enabled ( ) ;
}
// return true if we should use the range finder sensor
bool NavEKF2_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 ;
}
// return true if optical flow data is available
bool NavEKF2_core : : optFlowDataPresent ( void ) const
{
return ( imuSampleTime_ms - flowMeaTime_ms < 200 ) ;
}
// return true if the filter to be ready to use gps
bool NavEKF2_core : : readyToUseGPS ( void ) const
{
2016-07-17 01:27:37 -03:00
return validOrigin & & tiltAlignComplete & & yawAlignComplete & & gpsGoodToAlign & & ( frontend - > _fusionModeGPS ! = 3 ) & & gpsDataToFuse ;
2015-10-05 23:30:07 -03:00
}
2016-10-25 17:54:29 -03:00
// return true if the filter to be ready to use the beacon range measurements
bool NavEKF2_core : : readyToUseRangeBeacon ( void ) const
{
return tiltAlignComplete & & yawAlignComplete & & rngBcnGoodToAlign & & rngBcnDataToFuse ;
}
2018-03-07 00:42:31 -04:00
// return true if the filter to be ready to use external nav data
bool NavEKF2_core : : readyToUseExtNav ( void ) const
{
return tiltAlignComplete & & extNavDataToFuse ;
}
2015-10-05 23:30:07 -03:00
// return true if we should use the compass
bool NavEKF2_core : : use_compass ( void ) const
{
2015-11-15 17:05:08 -04:00
return _ahrs - > get_compass ( ) & & _ahrs - > get_compass ( ) - > use_for_yaw ( magSelectIndex ) & & ! allMagSensorsFailed ;
2015-10-05 23:30:07 -03:00
}
/*
should we assume zero sideslip ?
*/
bool NavEKF2_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
return _ahrs - > get_fly_forward ( ) & & _ahrs - > get_vehicle_class ( ) ! = AHRS_VEHICLE_GROUND ;
}
2015-10-07 15:27:09 -03:00
// set the LLH location of the filters NED origin
2017-04-19 03:29:17 -03:00
bool NavEKF2_core : : setOriginLLH ( const Location & loc )
2015-10-07 15:27:09 -03:00
{
2018-03-07 00:42:31 -04:00
if ( PV_AidingMode = = AID_ABSOLUTE & & ! extNavUsedForPos ) {
2015-10-07 15:27:09 -03:00
return false ;
}
EKF_origin = loc ;
2017-05-09 03:30:58 -03:00
ekfGpsRefHgt = ( double ) 0.01 * ( double ) EKF_origin . alt ;
2016-10-25 17:54:29 -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 ) ;
2015-10-07 15:27:09 -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 NavEKF2_core : : setOrigin ( const Location & loc )
2015-10-07 15:27:09 -03:00
{
2019-07-27 04:00:10 -03:00
EKF_origin = loc ;
2017-07-16 20:07:24 -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:30:58 -03:00
ekfGpsRefHgt = ( double ) 0.01 * ( double ) EKF_origin . alt ;
2015-10-07 15:27:09 -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 ) ;
2015-10-07 15:27:09 -03:00
validOrigin = true ;
2019-07-27 04:00:10 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " EKF2 IMU%u origin set " , ( unsigned ) imu_index ) ;
// 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 ;
2015-10-07 15:27:09 -03:00
}
2016-05-31 03:02:52 -03:00
// record a yaw reset event
void NavEKF2_core : : recordYawReset ( )
{
yawAlignComplete = true ;
if ( inFlight ) {
2016-06-09 20:19:28 -03:00
finalInflightYawInit = true ;
2016-05-31 03:02:52 -03:00
}
}
2016-07-06 02:33:29 -03:00
// return true and set the class variable true if the delta angle bias has been learned
bool NavEKF2_core : : checkGyroCalStatus ( void )
2016-07-04 20:12:59 -03:00
{
// check delta angle bias variances
2016-07-10 12:02:36 -03:00
const float delAngBiasVarMax = sq ( radians ( 0.15f * dtEkfAvg ) ) ;
2020-03-25 00:53:13 -03:00
if ( ! use_compass ( ) ) {
// rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a compass
// which can make this check fail
Vector3f delAngBiasVarVec = Vector3f ( P [ 9 ] [ 9 ] , P [ 10 ] [ 10 ] , P [ 11 ] [ 11 ] ) ;
Vector3f temp = prevTnb * delAngBiasVarVec ;
delAngBiasLearned = ( fabsf ( temp . x ) < delAngBiasVarMax ) & &
( fabsf ( temp . y ) < delAngBiasVarMax ) ;
} else {
delAngBiasLearned = ( P [ 9 ] [ 9 ] < = delAngBiasVarMax ) & &
( P [ 10 ] [ 10 ] < = delAngBiasVarMax ) & &
( P [ 11 ] [ 11 ] < = delAngBiasVarMax ) ;
}
2016-07-06 02:33:29 -03:00
return delAngBiasLearned ;
2016-07-04 20:12:59 -03:00
}
2015-10-07 15:27:09 -03:00
// Commands the EKF to not use GPS.
// This command must be sent prior to arming
// This command is forgotten by the EKF each time the vehicle disarms
// Returns 0 if command rejected
// Returns 1 if attitude, vertical velocity and vertical position will be provided
// Returns 2 if attitude, 3D-velocity, vertical position and relative horizontal position will be provided
uint8_t NavEKF2_core : : setInhibitGPS ( void )
{
2016-07-15 00:52:32 -03:00
if ( ( PV_AidingMode = = AID_ABSOLUTE ) & & motorsArmed ) {
2015-10-07 15:27:09 -03:00
return 0 ;
} else {
2016-07-15 00:09:48 -03:00
gpsInhibit = true ;
2015-10-07 15:27:09 -03:00
return 1 ;
}
2016-07-15 00:09:48 -03:00
// option 2 is not yet implemented as it requires a deeper integration of optical flow and GPS operation
2015-10-07 15:27:09 -03:00
}
2016-07-10 10:15:46 -03:00
// Update the filter status
void NavEKF2_core : : updateFilterStatus ( void )
{
// init return value
filterStatus . value = 0 ;
bool doingFlowNav = ( PV_AidingMode = = AID_RELATIVE ) & & flowDataValid ;
bool doingWindRelNav = ! tasTimeout & & assume_zero_sideslip ( ) ;
bool doingNormalGpsNav = ! posTimeout & & ( PV_AidingMode = = AID_ABSOLUTE ) ;
2018-10-03 23:43:13 -03:00
bool someVertRefData = ( ! velTimeout & & ( useGpsVertVel | | useExtNavVel ) ) | | ! hgtTimeout ;
2016-07-10 10:15:46 -03:00
bool someHorizRefData = ! ( velTimeout & & posTimeout & & tasTimeout ) | | doingFlowNav ;
2016-10-06 17:51:08 -03:00
bool optFlowNavPossible = flowDataValid & & delAngBiasLearned ;
2016-07-10 10:15:46 -03:00
bool gpsNavPossible = ! gpsNotAvailable & & gpsGoodToAlign & & delAngBiasLearned ;
bool filterHealthy = healthy ( ) & & tiltAlignComplete & & ( yawAlignComplete | | ( ! use_compass ( ) & & ( PV_AidingMode = = AID_NONE ) ) ) ;
// If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
bool hgtNotAccurate = ( frontend - > _altSource = = 2 ) & & ! validOrigin ;
// 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
filterStatus . flags . horiz_pos_rel = ( ( doingFlowNav & & gndOffsetValid ) | | doingWindRelNav | | doingNormalGpsNav ) & & filterHealthy ; // relative horizontal position estimate valid
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
filterStatus . flags . pred_horiz_pos_rel = ( ( optFlowNavPossible | | gpsNavPossible ) & & filterHealthy ) | | filterStatus . flags . horiz_pos_rel ; // we should be able to estimate a relative position when we enter flight mode
filterStatus . flags . pred_horiz_pos_abs = ( gpsNavPossible & & filterHealthy ) | | filterStatus . flags . horiz_pos_abs ; // we should be able to estimate an absolute position when we enter flight mode
filterStatus . flags . takeoff_detected = takeOffDetected ; // takeoff for optical flow navigation has been detected
filterStatus . flags . takeoff = expectGndEffectTakeoff ; // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
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 ) ;
2018-03-26 06:18:07 -03:00
filterStatus . flags . gps_glitching = ! gpsAccuracyGood & & ( PV_AidingMode = = AID_ABSOLUTE ) & & ! extNavUsedForPos ; // GPS glitching is affecting navigation accuracy
2018-07-13 21:10:10 -03:00
filterStatus . flags . gps_quality_good = gpsGoodToAlign ;
2020-02-07 16:33:40 -04:00
filterStatus . flags . initalized = filterStatus . flags . initalized | | healthy ( ) ;
2016-07-10 10:15:46 -03:00
}
2020-04-23 03:23:12 -03:00
void NavEKF2_core : : runYawEstimatorPrediction ( )
2020-03-25 00:53:13 -03:00
{
if ( yawEstimator ! = nullptr & & frontend - > _fusionModeGPS < = 1 ) {
float trueAirspeed ;
if ( is_positive ( defaultAirSpeed ) & & assume_zero_sideslip ( ) ) {
2020-04-20 21:32:39 -03:00
if ( imuDataDelayed . time_ms - tasDataDelayed . time_ms < 5000 ) {
2020-03-25 00:53:13 -03:00
trueAirspeed = tasDataDelayed . tas ;
} else {
trueAirspeed = defaultAirSpeed * AP : : ahrs ( ) . get_EAS2TAS ( ) ;
}
} else {
trueAirspeed = 0.0f ;
}
yawEstimator - > update ( imuDataDelayed . delAng , imuDataDelayed . delVel , imuDataDelayed . delAngDT , imuDataDelayed . delVelDT , EKFGSF_run_filterbank , trueAirspeed ) ;
2020-04-23 03:23:12 -03:00
}
}
2020-03-25 00:53:13 -03:00
2020-04-23 03:23:12 -03:00
void NavEKF2_core : : runYawEstimatorCorrection ( )
{
if ( yawEstimator ! = nullptr & & frontend - > _fusionModeGPS < = 1 ) {
2020-03-25 00:53:13 -03:00
if ( gpsDataToFuse ) {
Vector2f gpsVelNE = Vector2f ( gpsDataDelayed . vel . x , gpsDataDelayed . vel . y ) ;
float gpsVelAcc = fmaxf ( gpsSpdAccuracy , frontend - > _gpsHorizVelNoise ) ;
2020-04-24 18:52:45 -03:00
yawEstimator - > fuseVelData ( gpsVelNE , gpsVelAcc ) ;
2020-03-25 00:53:13 -03:00
}
// action an external reset request
2020-04-20 21:32:39 -03:00
if ( EKFGSF_yaw_reset_request_ms > 0 & & imuSampleTime_ms - EKFGSF_yaw_reset_request_ms < YAW_RESET_TO_GSF_TIMEOUT_MS ) {
2020-03-25 00:53:13 -03:00
EKFGSF_resetMainFilterYaw ( ) ;
}
}
}
// 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 NavEKF2_core : : EKFGSF_requestYawReset ( )
{
EKFGSF_yaw_reset_request_ms = imuSampleTime_ms ;
2020-07-29 01:16:27 -03:00
}