1 #include <AP_HAL/AP_HAL.h>
3 #include "AP_NavEKF3.h"
4 #include "AP_NavEKF3_core.h"
5 #include <GCS_MAVLink/GCS.h>
7 #include "AP_DAL/AP_DAL.h"
9 // Control filter mode transitions
10 void NavEKF3_core::controlFilterModes()
12 // Determine motor arm status
13 prevMotorsArmed
= motorsArmed
;
14 motorsArmed
= dal
.get_armed();
15 if (motorsArmed
&& !prevMotorsArmed
) {
16 // set the time at which we arm to assist with checks
17 timeAtArming_ms
= imuSampleTime_ms
;
20 // Detect if we are in flight on or ground
23 // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
24 // avoid unnecessary operations
25 setWindMagStateLearningMode();
27 // Check the alignmnent status of the tilt and yaw attitude
28 // Used during initial bootstrap alignment of the filter
29 checkAttitudeAlignmentStatus();
31 // Set the type of inertial navigation aiding used
37 return effective value for _magCal for this core
39 NavEKF3_core::MagCal
NavEKF3_core::effective_magCal(void) const
41 // force use of simple magnetic heading fusion for specified cores
42 if (frontend
->_magMask
& core_index
) {
46 // handle deprecated MagCal::EXTERNAL_YAW and MagCal::EXTERNAL_YAW_FALLBACK values
47 const int8_t magCalParamVal
= frontend
->_magCal
.get();
48 if (magCalParamVal
== 5) {
51 if (magCalParamVal
== 6) {
52 return MagCal::WHEN_FLYING
;
55 return MagCal(magCalParamVal
);
58 // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
59 // avoid unnecessary operations
60 void NavEKF3_core::setWindMagStateLearningMode()
62 const bool canEstimateWind
= ((finalInflightYawInit
&& dragFusionEnabled
) || assume_zero_sideslip()) &&
64 PV_AidingMode
!= AID_NONE
;
65 if (!inhibitWindStates
&& !canEstimateWind
) {
66 inhibitWindStates
= true;
67 updateStateIndexLim();
68 } else if (inhibitWindStates
&& canEstimateWind
&&
69 (sq(stateStruct
.velocity
.x
) + sq(stateStruct
.velocity
.y
) > sq(5.0f
) || dragFusionEnabled
)) {
70 inhibitWindStates
= false;
71 updateStateIndexLim();
72 // set states and variances
73 if (yawAlignComplete
&& assume_zero_sideslip()) {
74 // if we have a valid heading, set the wind states to the reciprocal of the vehicle heading
75 // which assumes the vehicle has launched into the wind
76 // use airspeed if if recent data available
78 stateStruct
.quat
.to_euler(tempEuler
.x
, tempEuler
.y
, tempEuler
.z
);
79 ftype trueAirspeedVariance
;
80 const bool haveAirspeedMeasurement
= usingDefaultAirspeed
|| (tasDataDelayed
.allowFusion
&& (imuDataDelayed
.time_ms
- tasDataDelayed
.time_ms
< 500) && useAirspeed());
81 if (haveAirspeedMeasurement
) {
82 trueAirspeedVariance
= constrain_ftype(tasDataDelayed
.tasVariance
, WIND_VEL_VARIANCE_MIN
, WIND_VEL_VARIANCE_MAX
);
83 const ftype windSpeed
= sqrtF(sq(stateStruct
.velocity
.x
) + sq(stateStruct
.velocity
.y
)) - tasDataDelayed
.tas
;
84 stateStruct
.wind_vel
.x
= windSpeed
* cosF(tempEuler
.z
);
85 stateStruct
.wind_vel
.y
= windSpeed
* sinF(tempEuler
.z
);
87 trueAirspeedVariance
= sq(WIND_VEL_VARIANCE_MAX
); // use 2-sigma for faster initial convergence
90 // set the wind state variances to the measurement uncertainty
93 P
[22][22] = P
[23][23] = trueAirspeedVariance
;
95 windStatesAligned
= true;
98 // set the variances using a typical max wind speed for small UAV operation
101 for (uint8_t index
=22; index
<=23; index
++) {
102 P
[index
][index
] = sq(WIND_VEL_VARIANCE_MAX
);
107 // determine if the vehicle is manoeuvring
108 manoeuvring
= accNavMagHoriz
> 0.5f
;
110 // Determine if learning of magnetic field states has been requested by the user
111 bool magCalRequested
=
112 ((effectiveMagCal
== MagCal::WHEN_FLYING
) && inFlight
) || // when flying
113 ((effectiveMagCal
== MagCal::WHEN_MANOEUVRING
) && manoeuvring
) || // when manoeuvring
114 ((effectiveMagCal
== MagCal::AFTER_FIRST_CLIMB
) && finalInflightYawInit
&& finalInflightMagInit
) || // when initial in-air yaw and mag field reset is complete
115 (effectiveMagCal
== MagCal::ALWAYS
); // all the time
117 // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
118 // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
119 bool magCalDenied
= !use_compass() || (effectiveMagCal
== MagCal::NEVER
) || (onGround
&& effectiveMagCal
!= MagCal::ALWAYS
);
121 // Inhibit the magnetic field calibration if not requested or denied
122 bool setMagInhibit
= !magCalRequested
|| magCalDenied
;
123 if (!inhibitMagStates
&& setMagInhibit
) {
124 inhibitMagStates
= true;
125 updateStateIndexLim();
126 // variances will be reset in CovariancePrediction
127 } else if (inhibitMagStates
&& !setMagInhibit
) {
128 inhibitMagStates
= false;
129 updateStateIndexLim();
130 if (magFieldLearned
) {
131 // if we have already learned the field states, then retain the learned variances
132 P
[16][16] = earthMagFieldVar
.x
;
133 P
[17][17] = earthMagFieldVar
.y
;
134 P
[18][18] = earthMagFieldVar
.z
;
135 P
[19][19] = bodyMagFieldVar
.x
;
136 P
[20][20] = bodyMagFieldVar
.y
;
137 P
[21][21] = bodyMagFieldVar
.z
;
139 // set the variances equal to the observation variances
140 for (uint8_t index
=16; index
<=21; index
++) {
141 P
[index
][index
] = sq(frontend
->_magNoise
);
144 // set the NE earth magnetic field states using the published declination
145 // and set the corresponding variances and covariances
146 alignMagStateDeclination();
149 // request a reset of the yaw and magnetic field states if not done before
150 if (!magStateInitComplete
|| (!finalInflightMagInit
&& inFlight
)) {
151 magYawResetRequest
= true;
155 // inhibit delta velocity bias learning if we have not yet aligned the tilt
156 if (tiltAlignComplete
&& inhibitDelVelBiasStates
) {
157 // activate the states
158 inhibitDelVelBiasStates
= false;
159 updateStateIndexLim();
161 // set the initial covariance values
162 P
[13][13] = sq(ACCEL_BIAS_LIM_SCALER
* frontend
->_accBiasLim
* dtEkfAvg
);
163 P
[14][14] = P
[13][13];
164 P
[15][15] = P
[13][13];
167 if (tiltAlignComplete
&& inhibitDelAngBiasStates
) {
168 // activate the states
169 inhibitDelAngBiasStates
= false;
170 updateStateIndexLim();
172 // set the initial covariance values
173 P
[10][10] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg
));
174 P
[11][11] = P
[10][10];
175 P
[12][12] = P
[10][10];
178 // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
179 // because we want it re-done for each takeoff
181 finalInflightYawInit
= false;
182 finalInflightMagInit
= false;
183 magFieldLearned
= false;
186 updateStateIndexLim();
189 // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
190 // if we are not using those states
191 void NavEKF3_core::updateStateIndexLim()
193 if (inhibitWindStates
) {
194 if (inhibitMagStates
) {
195 if (inhibitDelVelBiasStates
) {
196 if (inhibitDelAngBiasStates
) {
212 // Set inertial navigation aiding mode
213 void NavEKF3_core::setAidingMode()
215 resetDataSource posResetSource
= resetDataSource::DEFAULT
;
216 resetDataSource velResetSource
= resetDataSource::DEFAULT
;
218 // Save the previous status so we can detect when it has changed
219 PV_AidingModePrev
= PV_AidingMode
;
221 // Check that the gyro bias variance has converged
222 checkGyroCalStatus();
224 // Handle the special case where we are on ground and disarmed without a yaw measurement
225 // and navigating. This can occur if not using a magnetometer and yaw was aligned using GPS
226 // during the previous flight.
227 if (yaw_source_last
== AP_NavEKF_Source::SourceYaw::NONE
&&
230 PV_AidingMode
!= AID_NONE
)
232 PV_AidingMode
= AID_NONE
;
233 yawAlignComplete
= false;
234 finalInflightYawInit
= false;
235 ResetVelocity(resetDataSource::DEFAULT
);
236 ResetPosition(resetDataSource::DEFAULT
);
238 // preserve quaternion 4x4 covariances, but zero the other rows and columns
239 for (uint8_t row
=0; row
<4; row
++) {
240 for (uint8_t col
=4; col
<24; col
++) {
244 for (uint8_t col
=0; col
<4; col
++) {
245 for (uint8_t row
=4; row
<24; row
++) {
249 // keep the IMU bias state variances, but zero the covariances
250 ftype oldBiasVariance
[6];
251 for (uint8_t row
=0; row
<6; row
++) {
252 oldBiasVariance
[row
] = P
[row
+10][row
+10];
256 for (uint8_t row
=0; row
<6; row
++) {
257 P
[row
+10][row
+10] = oldBiasVariance
[row
];
261 // Determine if we should change aiding mode
262 switch (PV_AidingMode
) {
264 // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
265 // and IMU gyro bias estimates have stabilised
266 // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
267 // GPS aiding is the preferred option unless excluded by the user
268 if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) {
269 PV_AidingMode
= AID_ABSOLUTE
;
270 } else if (readyToUseOptFlow() || readyToUseBodyOdm()) {
271 PV_AidingMode
= AID_RELATIVE
;
276 // Check if the fusion has timed out (flow measurements have been rejected for too long)
277 bool flowFusionTimeout
= ((imuSampleTime_ms
- prevFlowFuseTime_ms
) > 5000);
278 // Check if the fusion has timed out (body odometry measurements have been rejected for too long)
279 bool bodyOdmFusionTimeout
= ((imuSampleTime_ms
- prevBodyVelFuseTime_ms
) > 5000);
280 // Enable switch to absolute position mode if GPS or range beacon data is available
281 // If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
282 if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) {
283 PV_AidingMode
= AID_ABSOLUTE
;
284 } else if (flowFusionTimeout
&& bodyOdmFusionTimeout
) {
285 PV_AidingMode
= AID_NONE
;
290 // Find the minimum time without data required to trigger any check
291 uint16_t minTestTime_ms
= MIN(frontend
->tiltDriftTimeMax_ms
, MIN(frontend
->posRetryTimeNoVel_ms
,frontend
->posRetryTimeUseVel_ms
));
293 // Check if optical flow data is being used
294 bool optFlowUsed
= (imuSampleTime_ms
- prevFlowFuseTime_ms
<= minTestTime_ms
);
296 // Check if body odometry data is being used
297 bool bodyOdmUsed
= (imuSampleTime_ms
- prevBodyVelFuseTime_ms
<= minTestTime_ms
);
299 // Check if airspeed data is being used
300 bool airSpdUsed
= (imuSampleTime_ms
- lastTasPassTime_ms
<= minTestTime_ms
);
302 // check if drag data is being used
303 bool dragUsed
= (imuSampleTime_ms
- lastDragPassTime_ms
<= minTestTime_ms
);
305 // Check if range beacon data is being used
306 bool rngBcnUsed
= (imuSampleTime_ms
- lastRngBcnPassTime_ms
<= minTestTime_ms
);
308 // Check if GPS or external nav is being used
309 bool posUsed
= (imuSampleTime_ms
- lastPosPassTime_ms
<= minTestTime_ms
);
310 bool gpsVelUsed
= (imuSampleTime_ms
- lastVelPassTime_ms
<= minTestTime_ms
);
312 // Check if attitude drift has been constrained by a measurement source
313 bool attAiding
= posUsed
|| gpsVelUsed
|| optFlowUsed
|| airSpdUsed
|| dragUsed
|| rngBcnUsed
|| bodyOdmUsed
;
315 // check if velocity drift has been constrained by a measurement source
316 bool velAiding
= gpsVelUsed
|| airSpdUsed
|| dragUsed
|| optFlowUsed
|| bodyOdmUsed
;
318 // check if position drift has been constrained by a measurement source
319 bool posAiding
= posUsed
|| rngBcnUsed
;
321 // Check if the loss of attitude aiding has become critical
322 bool attAidLossCritical
= false;
324 attAidLossCritical
= (imuSampleTime_ms
- prevFlowFuseTime_ms
> frontend
->tiltDriftTimeMax_ms
) &&
325 (imuSampleTime_ms
- lastTasPassTime_ms
> frontend
->tiltDriftTimeMax_ms
) &&
326 (imuSampleTime_ms
- lastRngBcnPassTime_ms
> frontend
->tiltDriftTimeMax_ms
) &&
327 (imuSampleTime_ms
- lastPosPassTime_ms
> frontend
->tiltDriftTimeMax_ms
) &&
328 (imuSampleTime_ms
- lastVelPassTime_ms
> frontend
->tiltDriftTimeMax_ms
);
331 // Check if the loss of position accuracy has become critical
332 bool posAidLossCritical
= false;
334 uint16_t maxLossTime_ms
;
336 maxLossTime_ms
= frontend
->posRetryTimeNoVel_ms
;
338 maxLossTime_ms
= frontend
->posRetryTimeUseVel_ms
;
340 posAidLossCritical
= (imuSampleTime_ms
- lastRngBcnPassTime_ms
> maxLossTime_ms
) &&
341 (imuSampleTime_ms
- lastPosPassTime_ms
> maxLossTime_ms
);
344 if (attAidLossCritical
) {
345 // if the loss of attitude data is critical, then put the filter into a constant position mode
346 PV_AidingMode
= AID_NONE
;
352 } else if (posAidLossCritical
) {
353 // if the loss of position is critical, declare all sources of position aiding as being timed out
355 velTimeout
= !optFlowUsed
&& !gpsVelUsed
&& !bodyOdmUsed
;
363 // check to see if we are starting or stopping aiding and set states and modes as required
364 if (PV_AidingMode
!= PV_AidingModePrev
) {
365 // set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
366 switch (PV_AidingMode
) {
368 // We have ceased aiding
369 GCS_SEND_TEXT(MAV_SEVERITY_WARNING
, "EKF3 IMU%u stopped aiding",(unsigned)imu_index
);
370 // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
373 // Reset the normalised innovation to avoid false failing bad fusion tests
376 // store the current position to be used to keep reporting the last known position
377 lastKnownPositionNE
.x
= stateStruct
.position
.x
;
378 lastKnownPositionNE
.y
= stateStruct
.position
.y
;
379 // initialise filtered altitude used to provide a takeoff reference to current baro on disarm
380 // this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
381 meaHgtAtTakeOff
= baroDataDelayed
.hgt
;
382 // reset the vertical position state to faster recover from baro errors experienced during touchdown
383 stateStruct
.position
.z
= -meaHgtAtTakeOff
;
384 // store the current height to be used to keep reporting
385 // the last known position
386 lastKnownPositionD
= stateStruct
.position
.z
;
387 // reset relative aiding sensor fusion activity status
388 flowFusionActive
= false;
389 bodyVelFusionActive
= false;
393 // We are doing relative position navigation where velocity errors are constrained, but position drift will occur
394 GCS_SEND_TEXT(MAV_SEVERITY_INFO
, "EKF3 IMU%u started relative aiding",(unsigned)imu_index
);
395 if (readyToUseOptFlow()) {
397 flowValidMeaTime_ms
= imuSampleTime_ms
;
398 prevFlowFuseTime_ms
= imuSampleTime_ms
;
399 } else if (readyToUseBodyOdm()) {
401 lastbodyVelPassTime_ms
= imuSampleTime_ms
;
402 prevBodyVelFuseTime_ms
= imuSampleTime_ms
;
409 if (readyToUseGPS()) {
410 // We are commencing aiding using GPS - this is the preferred method
411 posResetSource
= resetDataSource::GPS
;
412 velResetSource
= resetDataSource::GPS
;
413 GCS_SEND_TEXT(MAV_SEVERITY_INFO
, "EKF3 IMU%u is using GPS",(unsigned)imu_index
);
414 } else if (readyToUseRangeBeacon()) {
415 // We are commencing aiding using range beacons
416 posResetSource
= resetDataSource::RNGBCN
;
417 GCS_SEND_TEXT(MAV_SEVERITY_INFO
, "EKF3 IMU%u is using range beacons",(unsigned)imu_index
);
418 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
);
419 GCS_SEND_TEXT(MAV_SEVERITY_INFO
, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index
,(double)bcnPosOffsetNED
.z
);
420 #if EK3_FEATURE_EXTERNAL_NAV
421 } else if (readyToUseExtNav()) {
422 // we are commencing aiding using external nav
423 posResetSource
= resetDataSource::EXTNAV
;
424 GCS_SEND_TEXT(MAV_SEVERITY_INFO
, "EKF3 IMU%u is using external nav data",(unsigned)imu_index
);
425 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
);
427 velResetSource
= resetDataSource::EXTNAV
;
428 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
);
430 // handle height reset as special case
431 hgtMea
= -extNavDataDelayed
.pos
.z
;
432 posDownObsNoise
= sq(constrain_ftype(extNavDataDelayed
.posErr
, 0.1f
, 10.0f
));
434 #endif // EK3_FEATURE_EXTERNAL_NAV
437 // clear timeout flags as a precaution to avoid triggering any additional transitions
441 // reset the last fusion accepted times to prevent unwanted activation of timeout logic
442 lastPosPassTime_ms
= imuSampleTime_ms
;
443 lastVelPassTime_ms
= imuSampleTime_ms
;
444 lastRngBcnPassTime_ms
= imuSampleTime_ms
;
448 // Always reset the position and velocity when changing mode
449 ResetVelocity(velResetSource
);
450 ResetPosition(posResetSource
);
455 // Check the tilt and yaw alignmnent status
456 // Used during initial bootstrap alignment of the filter
457 void NavEKF3_core::checkAttitudeAlignmentStatus()
459 // Check for tilt convergence - used during initial alignment
460 // Once the tilt variances have reduced, re-set the yaw and magnetic field states
461 // and declare the tilt alignment complete
462 if (!tiltAlignComplete
) {
463 if (tiltErrorVariance
< sq(radians(5.0))) {
464 tiltAlignComplete
= true;
465 GCS_SEND_TEXT(MAV_SEVERITY_INFO
, "EKF3 IMU%u tilt alignment complete",(unsigned)imu_index
);
469 // submit yaw and magnetic field reset request
470 if (!yawAlignComplete
&& tiltAlignComplete
&& use_compass()) {
471 magYawResetRequest
= true;
476 // return true if we should use the airspeed sensor
477 bool NavEKF3_core::useAirspeed(void) const
479 return dal
.airspeed_sensor_enabled();
482 // return true if we should use the range finder sensor
483 bool NavEKF3_core::useRngFinder(void) const
485 // TO-DO add code to set this based in setting of optical flow use parameter and presence of sensor
489 // return true if the filter is ready to start using optical flow measurements
490 bool NavEKF3_core::readyToUseOptFlow(void) const
492 // ensure flow is used for navigation and not terrain alt estimation
493 if (frontend
->_flowUse
!= FLOW_USE_NAV
) {
497 if (!frontend
->sources
.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW
)) {
501 // We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use optical flow
502 return (imuSampleTime_ms
- flowMeaTime_ms
< 200) && tiltAlignComplete
&& delAngBiasLearned
;
505 // return true if the filter is ready to start using body frame odometry measurements
506 bool NavEKF3_core::readyToUseBodyOdm(void) const
508 #if EK3_FEATURE_BODY_ODOM
509 if (!frontend
->sources
.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV
) &&
510 !frontend
->sources
.useVelXYSource(AP_NavEKF_Source::SourceXY::WHEEL_ENCODER
)) {
511 // exit immediately if sources not configured to fuse external nav or wheel encoders
515 // Check for fresh visual odometry data that meets the accuracy required for alignment
516 bool visoDataGood
= (imuSampleTime_ms
- bodyOdmMeasTime_ms
< 200) && (bodyOdmDataNew
.velErr
< 1.0f
);
518 // Check for fresh wheel encoder data
519 bool wencDataGood
= (imuDataDelayed
.time_ms
- wheelOdmDataDelayed
.time_ms
< 200);
521 // We require stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use odometry measurements
522 // because they are in a body frame of reference
523 return (visoDataGood
|| wencDataGood
)
525 && delAngBiasLearned
;
528 #endif // EK3_FEATURE_BODY_ODOM
531 // return true if the filter to be ready to use gps
532 bool NavEKF3_core::readyToUseGPS(void) const
534 if (frontend
->sources
.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS
) {
538 return validOrigin
&& tiltAlignComplete
&& yawAlignComplete
&& (delAngBiasLearned
|| assume_zero_sideslip()) && gpsGoodToAlign
&& gpsDataToFuse
;
541 // return true if the filter to be ready to use the beacon range measurements
542 bool NavEKF3_core::readyToUseRangeBeacon(void) const
544 if (frontend
->sources
.getPosXYSource() != AP_NavEKF_Source::SourceXY::BEACON
) {
548 return tiltAlignComplete
&& yawAlignComplete
&& delAngBiasLearned
&& rngBcnAlignmentCompleted
&& rngBcnDataToFuse
;
551 // return true if the filter is ready to use external nav data
552 bool NavEKF3_core::readyToUseExtNav(void) const
554 #if EK3_FEATURE_EXTERNAL_NAV
555 if (frontend
->sources
.getPosXYSource() != AP_NavEKF_Source::SourceXY::EXTNAV
) {
559 return tiltAlignComplete
&& extNavDataToFuse
;
562 #endif // EK3_FEATURE_EXTERNAL_NAV
565 // return true if we should use the compass
566 bool NavEKF3_core::use_compass(void) const
568 const AP_NavEKF_Source::SourceYaw yaw_source
= frontend
->sources
.getYawSource();
569 if ((yaw_source
!= AP_NavEKF_Source::SourceYaw::COMPASS
) &&
570 (yaw_source
!= AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK
)) {
571 // not using compass as a yaw source
575 const auto &compass
= dal
.compass();
576 return compass
.use_for_yaw(magSelectIndex
) &&
577 !allMagSensorsFailed
;
580 // are we using (aka fusing) a non-compass yaw?
581 bool NavEKF3_core::using_noncompass_for_yaw(void) const
583 const AP_NavEKF_Source::SourceYaw yaw_source
= frontend
->sources
.getYawSource();
584 #if EK3_FEATURE_EXTERNAL_NAV
585 if (yaw_source
== AP_NavEKF_Source::SourceYaw::EXTNAV
) {
586 return ((imuSampleTime_ms
- last_extnav_yaw_fusion_ms
< 5000) || (imuSampleTime_ms
- lastSynthYawTime_ms
< 5000));
589 if (yaw_source
== AP_NavEKF_Source::SourceYaw::GPS
|| yaw_source
== AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK
||
590 yaw_source
== AP_NavEKF_Source::SourceYaw::GSF
|| !use_compass()) {
591 return imuSampleTime_ms
- last_gps_yaw_ms
< 5000 || imuSampleTime_ms
- lastSynthYawTime_ms
< 5000;
596 // are we using (aka fusing) external nav for yaw?
597 bool NavEKF3_core::using_extnav_for_yaw() const
599 #if EK3_FEATURE_EXTERNAL_NAV
600 if (frontend
->sources
.getYawSource() == AP_NavEKF_Source::SourceYaw::EXTNAV
) {
601 return ((imuSampleTime_ms
- last_extnav_yaw_fusion_ms
< 5000) || (imuSampleTime_ms
- lastSynthYawTime_ms
< 5000));
608 should we assume zero sideslip?
610 bool NavEKF3_core::assume_zero_sideslip(void) const
612 // we don't assume zero sideslip for ground vehicles as EKF could
613 // be quite sensitive to a rapid spin of the ground vehicle if
615 return dal
.get_fly_forward() && dal
.get_vehicle_class() != AP_DAL::VehicleClass::GROUND
;
618 // sets the local NED origin using a LLH location (latitude, longitude, height)
619 // returns false if absolute aiding and GPS is being used or if the origin is already set
620 bool NavEKF3_core::setOriginLLH(const Location
&loc
)
622 if ((PV_AidingMode
== AID_ABSOLUTE
) && (frontend
->sources
.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS
)) {
623 // reject attempts to set the origin if GPS is being used or if the origin is already set
627 return setOrigin(loc
);
630 // sets the local NED origin using a LLH location (latitude, longitude, height)
631 // returns false is the origin has already been set
632 bool NavEKF3_core::setOrigin(const Location
&loc
)
634 // if the origin is valid reject setting a new origin
640 ekfGpsRefHgt
= (double)0.01 * (double)EKF_origin
.alt
;
641 // define Earth rotation vector in the NED navigation frame at the origin
642 calcEarthRateNED(earthRateNED
, EKF_origin
.lat
);
644 GCS_SEND_TEXT(MAV_SEVERITY_INFO
, "EKF3 IMU%u origin set",(unsigned)imu_index
);
646 if (!frontend
->common_origin_valid
) {
647 frontend
->common_origin_valid
= true;
648 // put origin in frontend as well to ensure it stays in sync between lanes
649 public_origin
= EKF_origin
;
656 // record a yaw reset event
657 void NavEKF3_core::recordYawReset()
659 yawAlignComplete
= true;
661 finalInflightYawInit
= true;
665 // set the class variable true if the delta angle bias variances are sufficiently small
666 void NavEKF3_core::checkGyroCalStatus(void)
668 // check delta angle bias variances
669 const ftype delAngBiasVarMax
= sq(radians(0.15 * dtEkfAvg
));
670 const AP_NavEKF_Source::SourceYaw yaw_source
= frontend
->sources
.getYawSource();
671 if (!use_compass() && (yaw_source
!= AP_NavEKF_Source::SourceYaw::GPS
) && (yaw_source
!= AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK
) &&
672 (yaw_source
!= AP_NavEKF_Source::SourceYaw::EXTNAV
)) {
673 // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference
674 // which can make this check fail
675 Vector3F delAngBiasVarVec
= Vector3F(P
[10][10],P
[11][11],P
[12][12]);
676 Vector3F temp
= prevTnb
* delAngBiasVarVec
;
677 delAngBiasLearned
= (fabsF(temp
.x
) < delAngBiasVarMax
) &&
678 (fabsF(temp
.y
) < delAngBiasVarMax
);
680 delAngBiasLearned
= (P
[10][10] <= delAngBiasVarMax
) &&
681 (P
[11][11] <= delAngBiasVarMax
) &&
682 (P
[12][12] <= delAngBiasVarMax
);
686 // Update the filter status
687 void NavEKF3_core::updateFilterStatus(void)
690 filterStatus
.value
= 0;
691 bool doingBodyVelNav
= (PV_AidingMode
!= AID_NONE
) && (imuSampleTime_ms
- prevBodyVelFuseTime_ms
< 5000);
692 bool doingFlowNav
= (PV_AidingMode
!= AID_NONE
) && flowDataValid
;
693 bool doingWindRelNav
= (!tasTimeout
&& assume_zero_sideslip()) || !dragTimeout
;
694 bool doingNormalGpsNav
= !posTimeout
&& (PV_AidingMode
== AID_ABSOLUTE
);
695 bool someVertRefData
= (!velTimeout
&& (useGpsVertVel
|| useExtNavVel
)) || !hgtTimeout
;
696 bool someHorizRefData
= !(velTimeout
&& posTimeout
&& tasTimeout
&& dragTimeout
) || doingFlowNav
|| doingBodyVelNav
;
697 bool filterHealthy
= healthy() && tiltAlignComplete
&& (yawAlignComplete
|| (!use_compass() && (PV_AidingMode
!= AID_ABSOLUTE
)));
699 // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
700 bool hgtNotAccurate
= (frontend
->sources
.getPosZSource() == AP_NavEKF_Source::SourceZ::GPS
) && !validOrigin
;
702 // set individual flags
703 filterStatus
.flags
.attitude
= !stateStruct
.quat
.is_nan() && filterHealthy
; // attitude valid (we need a better check)
704 filterStatus
.flags
.horiz_vel
= someHorizRefData
&& filterHealthy
; // horizontal velocity estimate valid
705 filterStatus
.flags
.vert_vel
= someVertRefData
&& filterHealthy
; // vertical velocity estimate valid
706 filterStatus
.flags
.horiz_pos_rel
= ((doingFlowNav
&& gndOffsetValid
) || doingWindRelNav
|| doingNormalGpsNav
|| doingBodyVelNav
) && filterHealthy
; // relative horizontal position estimate valid
707 filterStatus
.flags
.horiz_pos_abs
= doingNormalGpsNav
&& filterHealthy
; // absolute horizontal position estimate valid
708 filterStatus
.flags
.vert_pos
= !hgtTimeout
&& filterHealthy
&& !hgtNotAccurate
; // vertical position estimate valid
709 filterStatus
.flags
.terrain_alt
= gndOffsetValid
&& filterHealthy
; // terrain height estimate valid
710 filterStatus
.flags
.const_pos_mode
= (PV_AidingMode
== AID_NONE
) && filterHealthy
; // constant position mode
711 filterStatus
.flags
.pred_horiz_pos_rel
= filterStatus
.flags
.horiz_pos_rel
; // EKF3 enters the required mode before flight
712 filterStatus
.flags
.pred_horiz_pos_abs
= filterStatus
.flags
.horiz_pos_abs
; // EKF3 enters the required mode before flight
713 filterStatus
.flags
.takeoff_detected
= takeOffDetected
; // takeoff for optical flow navigation has been detected
714 filterStatus
.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
715 filterStatus
.flags
.touchdown
= dal
.get_touchdown_expected(); // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
716 filterStatus
.flags
.using_gps
= ((imuSampleTime_ms
- lastPosPassTime_ms
) < 4000) && (PV_AidingMode
== AID_ABSOLUTE
);
717 filterStatus
.flags
.gps_glitching
= !gpsAccuracyGood
&& (PV_AidingMode
== AID_ABSOLUTE
) && (frontend
->sources
.getPosXYSource() == AP_NavEKF_Source::SourceXY::GPS
); // GPS glitching is affecting navigation accuracy
718 filterStatus
.flags
.gps_quality_good
= gpsGoodToAlign
;
719 // for reporting purposes we report rejecting airspeed after 3s of not fusing when we want to fuse the data
720 filterStatus
.flags
.rejecting_airspeed
= lastTasFailTime_ms
!= 0 &&
721 (imuSampleTime_ms
- lastTasFailTime_ms
) < 1000 &&
722 (imuSampleTime_ms
- lastTasPassTime_ms
) > 3000;
723 filterStatus
.flags
.initalized
= filterStatus
.flags
.initalized
|| healthy();
724 filterStatus
.flags
.dead_reckoning
= (PV_AidingMode
!= AID_NONE
) && doingWindRelNav
&& !((doingFlowNav
&& gndOffsetValid
) || doingNormalGpsNav
|| doingBodyVelNav
);
727 void NavEKF3_core::runYawEstimatorPrediction()
729 // exit immediately if no yaw estimator
730 if (yawEstimator
== nullptr) {
734 // ensure GPS is used for horizontal position and velocity
735 if (frontend
->sources
.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS
||
736 !frontend
->sources
.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS
)) {
741 if (tasDataDelayed
.allowFusion
&& assume_zero_sideslip()) {
742 trueAirspeed
= MAX(tasDataDelayed
.tas
, 0.0f
);
746 yawEstimator
->update(imuDataDelayed
.delAng
, imuDataDelayed
.delVel
, imuDataDelayed
.delAngDT
, imuDataDelayed
.delVelDT
, EKFGSF_run_filterbank
, trueAirspeed
);
749 void NavEKF3_core::runYawEstimatorCorrection()
751 // exit immediately if no yaw estimator
752 if (yawEstimator
== nullptr) {
755 // ensure GPS is used for horizontal position and velocity
756 if (frontend
->sources
.getPosXYSource() != AP_NavEKF_Source::SourceXY::GPS
||
757 !frontend
->sources
.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS
)) {
761 if (EKFGSF_run_filterbank
) {
763 Vector2F gpsVelNE
= Vector2F(gpsDataDelayed
.vel
.x
, gpsDataDelayed
.vel
.y
);
764 ftype gpsVelAcc
= fmaxF(gpsSpdAccuracy
, ftype(frontend
->_gpsHorizVelNoise
));
765 yawEstimator
->fuseVelData(gpsVelNE
, gpsVelAcc
);
767 // after velocity data has been fused the yaw variance estimate will have been refreshed and
768 // is used maintain a history of validity
769 ftype gsfYaw
, gsfYawVariance
;
770 if (EKFGSF_getYaw(gsfYaw
, gsfYawVariance
)) {
771 if (EKFGSF_yaw_valid_count
< GSF_YAW_VALID_HISTORY_THRESHOLD
) {
772 EKFGSF_yaw_valid_count
++;
775 EKFGSF_yaw_valid_count
= 0;
779 // action an external reset request
780 if (EKFGSF_yaw_reset_request_ms
> 0 && imuSampleTime_ms
- EKFGSF_yaw_reset_request_ms
< YAW_RESET_TO_GSF_TIMEOUT_MS
) {
781 EKFGSF_resetMainFilterYaw(true);
784 EKFGSF_yaw_valid_count
= 0;
788 // request a reset the yaw to the GSF estimate
789 // request times out after YAW_RESET_TO_GSF_TIMEOUT_MS if it cannot be actioned
790 void NavEKF3_core::EKFGSF_requestYawReset()
792 EKFGSF_yaw_reset_request_ms
= imuSampleTime_ms
;