Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #include "AP_NavEKF3.h"
4 : #include "AP_NavEKF3_core.h"
5 : #include <GCS_MAVLink/GCS.h>
6 :
7 : #include "AP_DAL/AP_DAL.h"
8 :
9 : // Control filter mode transitions
10 78317950 : void NavEKF3_core::controlFilterModes()
11 : {
12 : // Determine motor arm status
13 78317950 : prevMotorsArmed = motorsArmed;
14 78317950 : motorsArmed = dal.get_armed();
15 78317950 : if (motorsArmed && !prevMotorsArmed) {
16 : // set the time at which we arm to assist with checks
17 2122 : timeAtArming_ms = imuSampleTime_ms;
18 : }
19 :
20 : // Detect if we are in flight on or ground
21 78317950 : detectFlight();
22 :
23 : // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
24 : // avoid unnecessary operations
25 78317950 : setWindMagStateLearningMode();
26 :
27 : // Check the alignmnent status of the tilt and yaw attitude
28 : // Used during initial bootstrap alignment of the filter
29 78317950 : checkAttitudeAlignmentStatus();
30 :
31 : // Set the type of inertial navigation aiding used
32 78317950 : setAidingMode();
33 :
34 78317950 : }
35 :
36 : /*
37 : return effective value for _magCal for this core
38 : */
39 3144 : NavEKF3_core::MagCal NavEKF3_core::effective_magCal(void) const
40 : {
41 : // force use of simple magnetic heading fusion for specified cores
42 3144 : if (frontend->_magMask & core_index) {
43 : return MagCal::NEVER;
44 : }
45 :
46 : // handle deprecated MagCal::EXTERNAL_YAW and MagCal::EXTERNAL_YAW_FALLBACK values
47 3144 : const int8_t magCalParamVal = frontend->_magCal.get();
48 3144 : if (magCalParamVal == 5) {
49 : return MagCal::NEVER;
50 : }
51 3144 : if (magCalParamVal == 6) {
52 : return MagCal::WHEN_FLYING;
53 : }
54 :
55 3144 : return MagCal(magCalParamVal);
56 : }
57 :
58 : // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
59 : // avoid unnecessary operations
60 78317950 : void NavEKF3_core::setWindMagStateLearningMode()
61 : {
62 78317950 : const bool recentGpsYawFusion = (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS ||
63 377920 : yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) &&
64 78317950 : last_gps_yaw_fuse_ms != 0 &&
65 347302 : imuSampleTime_ms - last_gps_yaw_fuse_ms < 5000;
66 78003260 : const bool yawInitialised = recentGpsYawFusion || finalInflightYawInit;
67 78317950 : const bool canEstimateWind = ((yawInitialised && dragFusionEnabled) || assume_zero_sideslip()) &&
68 9427338 : !onGround &&
69 7493809 : PV_AidingMode != AID_NONE;
70 78317950 : if (!inhibitWindStates && !canEstimateWind) {
71 487 : inhibitWindStates = true;
72 487 : lastAspdEstIsValid = false;
73 487 : updateStateIndexLim();
74 78317463 : } else if (inhibitWindStates && canEstimateWind &&
75 217958 : (sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y) > sq(5.0f) || dragFusionEnabled)) {
76 511 : inhibitWindStates = false;
77 511 : updateStateIndexLim();
78 : // set states and variances
79 511 : if (yawAlignComplete && assume_zero_sideslip()) {
80 : // if we have a valid heading, set the wind states to the reciprocal of the vehicle heading
81 : // which assumes the vehicle has launched into the wind
82 : // use airspeed if if recent data available
83 499 : Vector3F tempEuler;
84 499 : stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
85 : ftype trueAirspeedVariance;
86 499 : const bool haveAirspeedMeasurement = (tasDataDelayed.allowFusion && (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 500) && useAirspeed());
87 : if (haveAirspeedMeasurement) {
88 485 : trueAirspeedVariance = constrain_ftype(tasDataDelayed.tasVariance, WIND_VEL_VARIANCE_MIN, WIND_VEL_VARIANCE_MAX);
89 485 : const ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
90 485 : stateStruct.wind_vel.x = windSpeed * cosF(tempEuler.z);
91 485 : stateStruct.wind_vel.y = windSpeed * sinF(tempEuler.z);
92 : } else {
93 14 : trueAirspeedVariance = sq(WIND_VEL_VARIANCE_MAX); // use 2-sigma for faster initial convergence
94 : }
95 :
96 : // set the wind state variances to the measurement uncertainty
97 499 : zeroStatesVarCov(22, 23);
98 499 : P[22][22] = P[23][23] = trueAirspeedVariance;
99 :
100 499 : windStatesAligned = true;
101 :
102 : } else {
103 : // set the variances using a typical max wind speed for small UAV operation
104 12 : zeroStatesVarCov(22, 23);
105 36 : for (uint8_t index=22; index<=23; index++) {
106 24 : P[index][index] = sq(WIND_VEL_VARIANCE_MAX);
107 : }
108 : }
109 : }
110 :
111 : // determine if the vehicle is manoeuvring
112 78317950 : manoeuvring = accNavMagHoriz > 0.5f;
113 :
114 : // Determine if learning of magnetic field states has been requested by the user
115 : bool magCalRequested =
116 78317950 : ((effectiveMagCal == MagCal::WHEN_FLYING) && inFlight) || // when flying
117 66633941 : ((effectiveMagCal == MagCal::WHEN_MANOEUVRING) && manoeuvring) || // when manoeuvring
118 66633941 : ((effectiveMagCal == MagCal::AFTER_FIRST_CLIMB) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete
119 108654873 : (effectiveMagCal == MagCal::ALWAYS) || // all the time
120 53200 : ((effectiveMagCal == MagCal::GROUND_AND_INFLIGHT) && (!inFlight || (finalInflightYawInit && finalInflightMagInit))); // on ground and after initial in-air yaw and mag field reset
121 :
122 : // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
123 : // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
124 78317950 : bool magCalDenied = !use_compass() || (effectiveMagCal == MagCal::NEVER) ||
125 76582823 : (onGround && effectiveMagCal != MagCal::ALWAYS && effectiveMagCal != MagCal::GROUND_AND_INFLIGHT);
126 :
127 : // Inhibit the magnetic field calibration if not requested or denied
128 78317950 : bool setMagInhibit = !magCalRequested || magCalDenied;
129 78317950 : if (!inhibitMagStates && setMagInhibit) {
130 1311 : inhibitMagStates = true;
131 1311 : updateStateIndexLim();
132 : // variances will be reset in CovariancePrediction
133 78316639 : } else if (inhibitMagStates && !setMagInhibit) {
134 1363 : inhibitMagStates = false;
135 1363 : updateStateIndexLim();
136 1363 : if (magFieldLearned) {
137 : // if we have already learned the field states, then retain the learned variances
138 884 : P[16][16] = earthMagFieldVar.x;
139 884 : P[17][17] = earthMagFieldVar.y;
140 884 : P[18][18] = earthMagFieldVar.z;
141 884 : P[19][19] = bodyMagFieldVar.x;
142 884 : P[20][20] = bodyMagFieldVar.y;
143 884 : P[21][21] = bodyMagFieldVar.z;
144 : } else {
145 : // set the variances equal to the observation variances
146 3353 : for (uint8_t index=16; index<=21; index++) {
147 2874 : P[index][index] = sq(frontend->_magNoise);
148 : }
149 :
150 : // set the NE earth magnetic field states using the published declination
151 : // and set the corresponding variances and covariances
152 479 : alignMagStateDeclination();
153 :
154 : }
155 : // request a reset of the yaw and magnetic field states if not done before
156 1363 : if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) {
157 479 : magYawResetRequest = true;
158 : }
159 : }
160 :
161 : // inhibit delta velocity bias learning if we have not yet aligned the tilt
162 78317950 : if (tiltAlignComplete && inhibitDelVelBiasStates) {
163 : // activate the states
164 2722 : inhibitDelVelBiasStates = false;
165 2722 : updateStateIndexLim();
166 :
167 : // set the initial covariance values
168 2722 : P[13][13] = sq(ACCEL_BIAS_LIM_SCALER * frontend->_accBiasLim * dtEkfAvg);
169 2722 : P[14][14] = P[13][13];
170 2722 : P[15][15] = P[13][13];
171 : }
172 :
173 78317950 : if (tiltAlignComplete && inhibitDelAngBiasStates) {
174 : // activate the states
175 2722 : inhibitDelAngBiasStates = false;
176 2722 : updateStateIndexLim();
177 :
178 : // set the initial covariance values
179 2722 : P[10][10] = sq(radians(InitialGyroBiasUncertainty() * dtEkfAvg));
180 2722 : P[11][11] = P[10][10];
181 2722 : P[12][12] = P[10][10];
182 : }
183 :
184 : // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
185 : // because we want it re-done for each takeoff
186 78317950 : if (onGround) {
187 24813474 : finalInflightYawInit = false;
188 24813474 : finalInflightMagInit = false;
189 24813474 : magFieldLearned = false;
190 : }
191 :
192 78317950 : updateStateIndexLim();
193 78317950 : }
194 :
195 : // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
196 : // if we are not using those states
197 78327066 : void NavEKF3_core::updateStateIndexLim()
198 : {
199 78327066 : if (inhibitWindStates) {
200 71053577 : if (inhibitMagStates) {
201 30322380 : if (inhibitDelVelBiasStates) {
202 1162599 : if (inhibitDelAngBiasStates) {
203 1162599 : stateIndexLim = 9;
204 : } else {
205 0 : stateIndexLim = 12;
206 : }
207 : } else {
208 29159781 : stateIndexLim = 15;
209 : }
210 : } else {
211 40731197 : stateIndexLim = 21;
212 : }
213 : } else {
214 7273489 : stateIndexLim = 23;
215 : }
216 78327066 : }
217 :
218 : // set the default yaw source
219 78317950 : void NavEKF3_core::setYawSource()
220 : {
221 78317950 : AP_NavEKF_Source::SourceYaw yaw_source = frontend->sources.getYawSource(core_index);
222 78317950 : if (wasLearningCompass_ms > 0) {
223 : // can't use compass while it is being calibrated
224 40059 : if (yaw_source == AP_NavEKF_Source::SourceYaw::COMPASS) {
225 : yaw_source = AP_NavEKF_Source::SourceYaw::NONE;
226 39371 : } else if (yaw_source == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) {
227 : yaw_source = AP_NavEKF_Source::SourceYaw::GPS;
228 : }
229 : }
230 78317950 : if (yaw_source != yaw_source_last) {
231 88 : yaw_source_last = yaw_source;
232 88 : yaw_source_reset = true;
233 : }
234 78317950 : }
235 :
236 : // Set inertial navigation aiding mode
237 78317950 : void NavEKF3_core::setAidingMode()
238 : {
239 : resetDataSource posResetSource = resetDataSource::DEFAULT;
240 : resetDataSource velResetSource = resetDataSource::DEFAULT;
241 :
242 : // Save the previous status so we can detect when it has changed
243 78317950 : PV_AidingModePrev = PV_AidingMode;
244 :
245 78317950 : setYawSource();
246 :
247 : // Check that the gyro bias variance has converged
248 78317950 : checkGyroCalStatus();
249 :
250 : // Handle the special case where we are on ground and disarmed without a yaw measurement
251 : // and in AID_ABSOLUTE mode. This can occur if not using a magnetometer and yaw was aligned
252 : // using GPS during the previous flight. AID_RELATIVE is excluded because optical flow and
253 : // body odometry are body-frame sensors that do not require yaw alignment.
254 78317950 : if (yaw_source_last == AP_NavEKF_Source::SourceYaw::NONE &&
255 158468 : !motorsArmed &&
256 38384 : onGround &&
257 38384 : PV_AidingMode == AID_ABSOLUTE)
258 : {
259 38 : PV_AidingMode = AID_NONE;
260 38 : yawAlignComplete = false;
261 38 : yawAlignGpsValidCount = 0;
262 38 : finalInflightYawInit = false;
263 38 : ResetVelocity(resetDataSource::DEFAULT);
264 38 : ResetPosition(resetDataSource::DEFAULT);
265 38 : ResetHeight();
266 : // preserve quaternion 4x4 covariances, but zero the other rows and columns
267 190 : for (uint8_t row=0; row<4; row++) {
268 3192 : for (uint8_t col=4; col<24; col++) {
269 3040 : P[row][col] = 0.0f;
270 : }
271 : }
272 190 : for (uint8_t col=0; col<4; col++) {
273 3192 : for (uint8_t row=4; row<24; row++) {
274 3040 : P[row][col] = 0.0f;
275 : }
276 : }
277 : // keep the IMU bias state variances, but zero the covariances
278 : ftype oldBiasVariance[6];
279 266 : for (uint8_t row=0; row<6; row++) {
280 228 : oldBiasVariance[row] = P[row+10][row+10];
281 : }
282 38 : zeroStatesVarCov(10, 15);
283 266 : for (uint8_t row=0; row<6; row++) {
284 228 : P[row+10][row+10] = oldBiasVariance[row];
285 : }
286 : }
287 :
288 : // Determine if we should change aiding mode
289 78317950 : switch (PV_AidingMode) {
290 16493864 : case AID_NONE: {
291 : // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
292 : // and IMU gyro bias estimates have stabilised
293 : // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
294 : // GPS aiding is the preferred option unless excluded by the user
295 16493864 : if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) {
296 1594 : PV_AidingMode = AID_ABSOLUTE;
297 : } else if (
298 : #if EK3_FEATURE_OPTFLOW_FUSION
299 32984522 : readyToUseOptFlow() ||
300 : #endif
301 16492252 : readyToUseBodyOdm()) {
302 24 : PV_AidingMode = AID_RELATIVE;
303 : }
304 : break;
305 : }
306 375746 : case AID_RELATIVE: {
307 : // Check if the fusion has timed out (flow measurements have been rejected for too long)
308 375746 : bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
309 : // Check if the fusion has timed out (body odometry measurements have been rejected for too long)
310 375746 : bool bodyOdmFusionTimeout = ((imuSampleTime_ms - prevBodyVelFuseTime_ms) > 5000);
311 : // Enable switch to absolute position mode if GPS or range beacon data is available
312 : // If GPS or range beacons data is not available and flow fusion has timed out, then fall-back to no-aiding
313 375746 : if (readyToUseGPS() || readyToUseRangeBeacon() || readyToUseExtNav()) {
314 8 : PV_AidingMode = AID_ABSOLUTE;
315 375738 : } else if (flowFusionTimeout && bodyOdmFusionTimeout) {
316 0 : PV_AidingMode = AID_NONE;
317 : }
318 : break;
319 : }
320 61448340 : case AID_ABSOLUTE: {
321 : // Find the minimum time without data required to trigger any check
322 61448340 : uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms));
323 :
324 : // Check if optical flow data is being used
325 61448340 : bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms);
326 :
327 : // Check if body odometry data is being used
328 61448340 : bool bodyOdmUsed = (imuSampleTime_ms - prevBodyVelFuseTime_ms <= minTestTime_ms);
329 :
330 : // Check if airspeed data is being used
331 61448340 : bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms);
332 :
333 : // check if drag data is being used
334 61448340 : bool dragUsed = (imuSampleTime_ms - lastDragPassTime_ms <= minTestTime_ms);
335 :
336 : #if EK3_FEATURE_BEACON_FUSION
337 : // Check if range beacon data is being used
338 61448340 : const bool rngBcnUsed = (imuSampleTime_ms - rngBcn.lastPassTime_ms <= minTestTime_ms);
339 : #else
340 : const bool rngBcnUsed = false;
341 : #endif
342 :
343 : // Check if GPS or external nav is being used
344 61448340 : bool posUsed = (imuSampleTime_ms - lastGpsPosPassTime_ms <= minTestTime_ms);
345 61448340 : bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
346 :
347 : // Check if attitude drift has been constrained by a measurement source
348 61448340 : bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed;
349 :
350 : // Check if velocity drift has been constrained by a measurement source
351 : // Currently these are all the same source as will stabilise attitude because we do not currently have
352 : // a sensor that only observes attitude
353 61448340 : velAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || dragUsed || rngBcnUsed || bodyOdmUsed;
354 :
355 : // Store the last valid airspeed estimate
356 61448340 : windStateIsObservable = !inhibitWindStates && (posUsed || gpsVelUsed || optFlowUsed || rngBcnUsed || bodyOdmUsed);
357 61448340 : if (windStateIsObservable) {
358 7181751 : lastAirspeedEstimate = (stateStruct.velocity - Vector3F(stateStruct.wind_vel.x, stateStruct.wind_vel.y, 0.0F)).length();
359 7181751 : lastAspdEstIsValid = true;
360 : }
361 :
362 : // check if position drift has been constrained by a measurement source
363 61448340 : bool posAiding = posUsed || rngBcnUsed;
364 :
365 : // Check if the loss of attitude aiding has become critical
366 : bool attAidLossCritical = false;
367 61448340 : if (!attAiding) {
368 118284 : attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) &&
369 59142 : (imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
370 : #if EK3_FEATURE_BEACON_FUSION
371 59123 : (imuSampleTime_ms - rngBcn.lastPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
372 : #endif
373 118148 : (imuSampleTime_ms - lastGpsPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
374 22 : (imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
375 : }
376 :
377 : // Check if the loss of position accuracy has become critical
378 : bool posAidLossCritical = false;
379 61448340 : if (!posAiding) {
380 : uint16_t maxLossTime_ms;
381 283301 : if (!velAiding) {
382 59142 : maxLossTime_ms = frontend->posRetryTimeNoVel_ms;
383 : } else {
384 224159 : maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
385 : }
386 : posAidLossCritical =
387 : #if EK3_FEATURE_BEACON_FUSION
388 283301 : (imuSampleTime_ms - rngBcn.lastPassTime_ms > maxLossTime_ms) &&
389 : #endif
390 282116 : (imuSampleTime_ms - lastGpsPosPassTime_ms > maxLossTime_ms);
391 : }
392 :
393 61448340 : if (attAidLossCritical) {
394 : // if the loss of attitude data is critical, then put the filter into a constant position mode
395 22 : PV_AidingMode = AID_NONE;
396 22 : posTimeout = true;
397 22 : velTimeout = true;
398 22 : tasTimeout = true;
399 22 : dragTimeout = true;
400 22 : gpsIsInUse = false;
401 61448318 : } else if (posAidLossCritical) {
402 : // if the loss of position is critical, declare all sources of position aiding as being timed out
403 224271 : posTimeout = true;
404 224271 : velTimeout = !optFlowUsed && !gpsVelUsed && !bodyOdmUsed;
405 224271 : gpsIsInUse = false;
406 :
407 : }
408 : break;
409 : }
410 : }
411 :
412 : // check to see if we are starting or stopping aiding and set states and modes as required
413 78317950 : if (PV_AidingMode != PV_AidingModePrev) {
414 : // set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
415 1686 : switch (PV_AidingMode) {
416 60 : case AID_NONE:
417 : // We have ceased aiding
418 60 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF3 IMU%u stopped aiding",(unsigned)imu_index);
419 : // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
420 60 : posTimeout = true;
421 60 : velTimeout = true;
422 : // Reset the normalised innovation to avoid false failing bad fusion tests
423 60 : velTestRatio = 0.0f;
424 60 : posTestRatio = 0.0f;
425 : // store the current position to be used to keep reporting the last known position
426 60 : lastKnownPositionNE.x = stateStruct.position.x;
427 60 : lastKnownPositionNE.y = stateStruct.position.y;
428 : // initialise filtered altitude used to provide a takeoff reference to current baro on disarm
429 : // this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
430 60 : meaHgtAtTakeOff = baroDataDelayed.hgt;
431 : // reset the vertical position state to faster recover from baro errors experienced during touchdown
432 60 : stateStruct.position.z = -meaHgtAtTakeOff;
433 : // store the current height to be used to keep reporting
434 : // the last known position
435 60 : lastKnownPositionD = stateStruct.position.z;
436 : // reset relative aiding sensor fusion activity status
437 60 : flowFusionActive = false;
438 60 : bodyVelFusionActive = false;
439 60 : break;
440 :
441 24 : case AID_RELATIVE:
442 : // We are doing relative position navigation where velocity errors are constrained, but position drift will occur
443 24 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u started relative aiding",(unsigned)imu_index);
444 : #if EK3_FEATURE_OPTFLOW_FUSION
445 24 : if (readyToUseOptFlow()) {
446 : // Reset time stamps
447 18 : flowValidMeaTime_ms = imuSampleTime_ms;
448 18 : prevFlowFuseTime_ms = imuSampleTime_ms;
449 : } else
450 : #endif
451 6 : if (readyToUseBodyOdm()) {
452 : // Reset time stamps
453 6 : lastbodyVelPassTime_ms = imuSampleTime_ms;
454 6 : prevBodyVelFuseTime_ms = imuSampleTime_ms;
455 : }
456 24 : posTimeout = true;
457 24 : velTimeout = true;
458 24 : break;
459 :
460 1602 : case AID_ABSOLUTE:
461 1602 : if (readyToUseGPS()) {
462 : // We are commencing aiding using GPS - this is the preferred method
463 : posResetSource = resetDataSource::GPS;
464 : velResetSource = resetDataSource::GPS;
465 1591 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u is using GPS",(unsigned)imu_index);
466 : #if EK3_FEATURE_BEACON_FUSION
467 11 : } else if (readyToUseRangeBeacon()) {
468 : // We are commencing aiding using range beacons
469 : posResetSource = resetDataSource::RNGBCN;
470 6 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u is using range beacons",(unsigned)imu_index);
471 6 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u initial pos NE = %3.1f,%3.1f (m)",(unsigned)imu_index,(double)rngBcn.receiverPos.x,(double)rngBcn.receiverPos.y);
472 6 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)rngBcn.posOffsetNED.z);
473 : #endif // EK3_FEATURE_BEACON_FUSION
474 : #if EK3_FEATURE_EXTERNAL_NAV
475 5 : } else if (readyToUseExtNav()) {
476 : // we are commencing aiding using external nav
477 : posResetSource = resetDataSource::EXTNAV;
478 5 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u is using external nav data",(unsigned)imu_index);
479 5 : 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);
480 5 : if (useExtNavVel) {
481 : velResetSource = resetDataSource::EXTNAV;
482 5 : 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);
483 : }
484 : // handle height reset as special case
485 5 : if (frontend->sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::EXTNAV) {
486 3 : hgtMea = -extNavDataDelayed.pos.z;
487 3 : posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.1f, 10.0f));
488 3 : ResetHeight();
489 : }
490 : #endif // EK3_FEATURE_EXTERNAL_NAV
491 : }
492 :
493 : // clear timeout flags as a precaution to avoid triggering any additional transitions
494 1602 : posTimeout = false;
495 1602 : velTimeout = false;
496 :
497 : // reset the last fusion accepted times to prevent unwanted activation of timeout logic
498 1602 : lastGpsPosPassTime_ms = imuSampleTime_ms;
499 1602 : lastVelPassTime_ms = imuSampleTime_ms;
500 : #if EK3_FEATURE_BEACON_FUSION
501 1602 : rngBcn.lastPassTime_ms = imuSampleTime_ms;
502 : #endif
503 1602 : break;
504 : }
505 :
506 : // Always reset the position and velocity when changing mode
507 1686 : ResetVelocity(velResetSource);
508 1686 : ResetPosition(posResetSource);
509 : }
510 :
511 78317950 : }
512 :
513 : // Check the tilt and yaw alignmnent status
514 : // Used during initial bootstrap alignment of the filter
515 78317950 : void NavEKF3_core::checkAttitudeAlignmentStatus()
516 : {
517 : // Check for tilt convergence - used during initial alignment
518 : // Once the tilt variances have reduced, re-set the yaw and magnetic field states
519 : // and declare the tilt alignment complete
520 78317950 : if (!tiltAlignComplete) {
521 1164935 : if (tiltErrorVariance < sq(radians(5.0))) {
522 2722 : tiltAlignComplete = true;
523 2722 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u tilt alignment complete",(unsigned)imu_index);
524 : }
525 : }
526 :
527 : // submit yaw and magnetic field reset request
528 78317950 : if (!yawAlignComplete && tiltAlignComplete && use_compass()) {
529 74215 : magYawResetRequest = true;
530 : }
531 :
532 78317950 : }
533 :
534 : // return true if we should use the airspeed sensor
535 18761723 : bool NavEKF3_core::useAirspeed(void) const
536 : {
537 18761723 : return dal.airspeed_sensor_enabled();
538 : }
539 :
540 : // return true if we should use the range finder sensor
541 0 : bool NavEKF3_core::useRngFinder(void) const
542 : {
543 : // TO-DO add code to set this based in setting of optical flow use parameter and presence of sensor
544 0 : return true;
545 : }
546 :
547 : #if EK3_FEATURE_OPTFLOW_FUSION
548 : // return true if the filter is ready to start using optical flow measurements
549 16492294 : bool NavEKF3_core::readyToUseOptFlow(void) const
550 : {
551 : // ensure flow is used for navigation and not terrain alt estimation
552 16492294 : if (frontend->_flowUse != FLOW_USE_NAV) {
553 : return false;
554 : }
555 :
556 15075173 : if (!frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::OPTFLOW, core_index)) {
557 : return false;
558 : }
559 :
560 : // We need stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use optical flow
561 10163 : return (imuSampleTime_ms - flowMeaTime_ms < 200) && tiltAlignComplete && delAngBiasLearned;
562 : }
563 : #endif // EK3_FEATURE_OPTFLOW_FUSION
564 :
565 : // return true if the filter is ready to start using body frame odometry measurements
566 16492258 : bool NavEKF3_core::readyToUseBodyOdm(void) const
567 : {
568 : #if EK3_FEATURE_BODY_ODOM
569 32969437 : if (!frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::EXTNAV, core_index) &&
570 16477179 : !frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::WHEEL_ENCODER, core_index)) {
571 : // exit immediately if sources not configured to fuse external nav or wheel encoders
572 : return false;
573 : }
574 :
575 : // Check for fresh visual odometry data that meets the accuracy required for alignment
576 15079 : bool visoDataGood = (imuSampleTime_ms - bodyOdmMeasTime_ms < 200) && (bodyOdmDataNew.velErr < 1.0f);
577 :
578 : // Check for fresh wheel encoder data
579 15079 : bool wencDataGood = (imuDataDelayed.time_ms - wheelOdmDataDelayed.time_ms < 200);
580 :
581 : // We require stable roll/pitch angles and gyro bias estimates but do not need the yaw angle aligned to use odometry measurements
582 : // because they are in a body frame of reference
583 15079 : return (visoDataGood || wencDataGood)
584 1588 : && tiltAlignComplete
585 15091 : && delAngBiasLearned;
586 : #else
587 : return false;
588 : #endif // EK3_FEATURE_BODY_ODOM
589 : }
590 :
591 : // return true if the filter to be ready to use gps
592 16871212 : bool NavEKF3_core::readyToUseGPS(void) const
593 : {
594 16871212 : if (frontend->sources.getPosXYSource(core_index) != AP_NavEKF_Source::SourceXY::GPS) {
595 : return false;
596 : }
597 :
598 16425447 : return validOrigin && tiltAlignComplete && yawAlignComplete && (delAngBiasLearned || assume_zero_sideslip()) && gpsGoodToAlign && gpsDataToFuse;
599 : }
600 :
601 : // return true if the filter to be ready to use the beacon range measurements
602 16868030 : bool NavEKF3_core::readyToUseRangeBeacon(void) const
603 : {
604 : #if EK3_FEATURE_BEACON_FUSION
605 16868030 : if (frontend->sources.getPosXYSource(core_index) != AP_NavEKF_Source::SourceXY::BEACON) {
606 : return false;
607 : }
608 :
609 62891 : return tiltAlignComplete && yawAlignComplete && delAngBiasLearned && rngBcn.alignmentCompleted && rngBcn.dataToFuse;
610 : #else
611 : return false;
612 : #endif // EK3_FEATURE_BEACON_FUSION
613 : }
614 :
615 : // return true if the filter is ready to use external nav data
616 16868018 : bool NavEKF3_core::readyToUseExtNav(void) const
617 : {
618 : #if EK3_FEATURE_EXTERNAL_NAV
619 16868018 : if (frontend->sources.getPosXYSource(core_index) != AP_NavEKF_Source::SourceXY::EXTNAV) {
620 : return false;
621 : }
622 :
623 29494 : return tiltAlignComplete && extNavDataToFuse;
624 : #else
625 : return false;
626 : #endif // EK3_FEATURE_EXTERNAL_NAV
627 : }
628 :
629 : // return true if we should use the compass
630 259350736 : bool NavEKF3_core::use_compass(void) const
631 : {
632 259350736 : if ((yaw_source_last != AP_NavEKF_Source::SourceYaw::COMPASS) &&
633 : (yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK)) {
634 : // not using compass as a yaw source
635 : return false;
636 : }
637 :
638 256219627 : const auto &compass = dal.compass();
639 256219627 : return compass.use_for_yaw(magSelectIndex) &&
640 256219602 : !allMagSensorsFailed;
641 : }
642 :
643 : // are we using (aka fusing) a non-compass yaw?
644 40189721 : bool NavEKF3_core::using_noncompass_for_yaw(void) const
645 : {
646 : #if EK3_FEATURE_EXTERNAL_NAV
647 40189721 : if (yaw_source_last == AP_NavEKF_Source::SourceYaw::EXTNAV) {
648 97068 : return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000));
649 : }
650 : #endif
651 40092819 : if (yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS || yaw_source_last == AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK ||
652 40092819 : yaw_source_last == AP_NavEKF_Source::SourceYaw::GSF || !use_compass()) {
653 1201801 : return imuSampleTime_ms - last_gps_yaw_ms < 5000 || imuSampleTime_ms - lastSynthYawTime_ms < 5000;
654 : }
655 : return false;
656 : }
657 :
658 : // are we using (aka fusing) external nav for yaw?
659 40053019 : bool NavEKF3_core::using_extnav_for_yaw() const
660 : {
661 : #if EK3_FEATURE_EXTERNAL_NAV
662 40053019 : if (yaw_source_last == AP_NavEKF_Source::SourceYaw::EXTNAV) {
663 68739 : return ((imuSampleTime_ms - last_extnav_yaw_fusion_ms < 5000) || (imuSampleTime_ms - lastSynthYawTime_ms < 5000));
664 : }
665 : #endif
666 : return false;
667 : }
668 :
669 : // are we using a gps
670 40053019 : bool NavEKF3_core::using_gps() const
671 : {
672 40053019 : return frontend->sources.usingGPS(core_index);
673 : }
674 :
675 : /*
676 : should we assume zero sideslip?
677 : */
678 291001766 : bool NavEKF3_core::assume_zero_sideslip(void) const
679 : {
680 : // we don't assume zero sideslip for ground vehicles as EKF could
681 : // be quite sensitive to a rapid spin of the ground vehicle if
682 : // traction is lost
683 291001766 : return dal.get_fly_forward() && dal.get_vehicle_class() != AP_DAL::VehicleClass::GROUND;
684 : }
685 :
686 : // populates the Earth magnetic field table using the given location
687 1898 : void NavEKF3_core::setEarthFieldFromLocation(const Location &loc)
688 : {
689 1898 : const auto &compass = dal.compass();
690 1898 : if (compass.have_scale_factor(magSelectIndex) &&
691 1726 : compass.auto_declination_enabled()) {
692 1726 : getEarthFieldTable(loc);
693 1726 : if (frontend->_mag_ef_limit > 0) {
694 : // initialise earth field from tables
695 1726 : stateStruct.earth_magfield = table_earth_field_ga;
696 : }
697 : }
698 1898 : }
699 :
700 : // sets the local NED origin using a LLH location (latitude, longitude, height)
701 : // returns false is the origin has already been set
702 1734 : bool NavEKF3_core::setOriginLLH(const Location &loc)
703 : {
704 : // if the origin is valid reject setting a new origin
705 1734 : if (validOrigin) {
706 : return false;
707 : }
708 :
709 1734 : EKF_origin = loc;
710 1734 : ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
711 : // define Earth rotation vector in the NED navigation frame at the origin
712 1734 : calcEarthRateNED(earthRateNED, EKF_origin.lat);
713 1734 : validOrigin = true;
714 :
715 : // but we do want to populate the WMM table even if we don't have a GPS at all
716 1734 : if (!stateStruct.quat.is_zero()) {
717 1728 : alignMagStateDeclination();
718 1728 : setEarthFieldFromLocation(EKF_origin);
719 : }
720 :
721 1734 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u origin set",(unsigned)imu_index);
722 :
723 1734 : if (!frontend->common_origin_valid) {
724 866 : frontend->common_origin_valid = true;
725 : // put origin in frontend as well to ensure it stays in sync between lanes
726 866 : public_origin = EKF_origin;
727 : }
728 :
729 :
730 : return true;
731 : }
732 :
733 : // record all requested yaw resets completed
734 4547 : void NavEKF3_core::recordYawResetsCompleted()
735 : {
736 4547 : gpsYawResetRequest = false;
737 4547 : magYawResetRequest = false;
738 4547 : yawAlignComplete = true;
739 4547 : if (inFlight) {
740 1381 : finalInflightYawInit = true;
741 : }
742 4547 : }
743 :
744 : // set the class variable true if the delta angle bias variances are sufficiently small
745 78317950 : void NavEKF3_core::checkGyroCalStatus(void)
746 : {
747 : // check delta angle bias variances
748 78317950 : const ftype delAngBiasVarMax = sq(radians(0.15 * dtEkfAvg));
749 78317950 : if (!use_compass() && (yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS) && (yaw_source_last != AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK) &&
750 : (yaw_source_last != AP_NavEKF_Source::SourceYaw::EXTNAV)) {
751 : // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a yaw reference
752 : // which can make this check fail
753 197181 : const Vector3F delAngBiasVarVec { P[10][10], P[11][11], P[12][12] };
754 197181 : const Vector3F temp = prevTnb * delAngBiasVarVec;
755 394362 : delAngBiasLearned = (fabsF(temp.x) < delAngBiasVarMax) &&
756 170632 : (fabsF(temp.y) < delAngBiasVarMax);
757 : } else {
758 78120769 : delAngBiasLearned = (P[10][10] <= delAngBiasVarMax) &&
759 78120769 : (P[11][11] <= delAngBiasVarMax) &&
760 72504681 : (P[12][12] <= delAngBiasVarMax);
761 : }
762 78317950 : }
763 :
764 : // Update the filter status
765 19965274 : void NavEKF3_core::updateFilterStatus(void)
766 : {
767 : // init return value
768 : nav_filter_status status;
769 19965274 : status.value = 0;
770 19965274 : bool doingBodyVelNav = (PV_AidingMode != AID_NONE) && (imuSampleTime_ms - prevBodyVelFuseTime_ms < 5000);
771 19965274 : bool doingFlowNav = (PV_AidingMode != AID_NONE) && flowDataValid;
772 19965274 : bool doingWindRelNav = (!tasTimeout && assume_zero_sideslip()) || !dragTimeout;
773 19965274 : bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
774 19965274 : bool someVertRefData = (!velTimeout && (useGpsVertVel || useExtNavVel)) || !hgtTimeout;
775 19965274 : bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout && dragTimeout) || doingFlowNav || doingBodyVelNav;
776 19965274 : bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode != AID_ABSOLUTE)));
777 :
778 : // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
779 19965274 : bool hgtNotAccurate = (frontend->sources.getPosZSource(core_index) == AP_NavEKF_Source::SourceZ::GPS) && !validOrigin;
780 :
781 : // set individual flags
782 19965274 : status.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
783 19965274 : status.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid
784 19965274 : status.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
785 :
786 : #if EK3_FEATURE_OPTFLOW_SRTM
787 19965274 : const bool optflow_gnd_offset = gndOffsetValid || terrain_srtm_alt_valid;
788 : #else
789 : const bool optflow_gnd_offset = gndOffsetValid;
790 : #endif
791 19965274 : status.flags.horiz_pos_rel = ((doingFlowNav && optflow_gnd_offset) || doingWindRelNav || doingNormalGpsNav || doingBodyVelNav) && filterHealthy; // relative horizontal position estimate valid
792 :
793 19965274 : status.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
794 19965274 : status.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
795 19965274 : status.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
796 19965274 : status.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
797 19965274 : status.flags.pred_horiz_pos_rel = status.flags.horiz_pos_rel; // EKF3 enters the required mode before flight
798 19965274 : status.flags.pred_horiz_pos_abs = status.flags.horiz_pos_abs; // EKF3 enters the required mode before flight
799 19965274 : status.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
800 19965274 : status.flags.takeoff = dal.get_takeoff_expected(); // The EKF has been told to expect takeoff is in a ground effect mitigation mode and has started the EKF-GSF yaw estimator
801 19965274 : status.flags.touchdown = dal.get_touchdown_expected(); // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
802 19965274 : status.flags.using_gps = ((imuSampleTime_ms - lastGpsPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
803 19965274 : status.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && (frontend->sources.getPosXYSource(core_index) == AP_NavEKF_Source::SourceXY::GPS); // GPS glitching is affecting navigation accuracy
804 19965274 : status.flags.gps_quality_good = gpsGoodToAlign;
805 : // for reporting purposes we report rejecting airspeed after 3s of not fusing when we want to fuse the data
806 39976983 : status.flags.rejecting_airspeed = lastTasFailTime_ms != 0 &&
807 19965274 : (imuSampleTime_ms - lastTasFailTime_ms) < 1000 &&
808 9339 : (imuSampleTime_ms - lastTasPassTime_ms) > 3000;
809 19965274 : status.flags.initalized = status.flags.initalized || healthy();
810 19965274 : status.flags.dead_reckoning = (PV_AidingMode != AID_NONE) && doingWindRelNav && !((doingFlowNav && gndOffsetValid) || doingNormalGpsNav || doingBodyVelNav);
811 :
812 19965274 : filterStatus.value = status.value;
813 19965274 : }
814 :
815 19965274 : void NavEKF3_core::runYawEstimatorPrediction()
816 : {
817 : // exit immediately if no yaw estimator
818 19965274 : if (yawEstimator == nullptr) {
819 : return;
820 : }
821 :
822 : // ensure GPS is used for horizontal position and velocity
823 39737603 : if (frontend->sources.getPosXYSource(core_index) != AP_NavEKF_Source::SourceXY::GPS ||
824 19780419 : !frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS, core_index)) {
825 199774 : return;
826 : }
827 :
828 : ftype trueAirspeed;
829 19757410 : if (tasDataDelayed.allowFusion && assume_zero_sideslip()) {
830 4420593 : trueAirspeed = MAX(tasDataDelayed.tas, 0.0f);
831 : } else {
832 : trueAirspeed = 0.0f;
833 : }
834 19757410 : yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed);
835 : }
836 :
837 19965274 : void NavEKF3_core::runYawEstimatorCorrection()
838 : {
839 : // exit immediately if no yaw estimator
840 19965274 : if (yawEstimator == nullptr) {
841 : return;
842 : }
843 : // ensure GPS is used for horizontal position and velocity
844 39737603 : if (frontend->sources.getPosXYSource(core_index) != AP_NavEKF_Source::SourceXY::GPS ||
845 19780419 : !frontend->sources.useVelXYSource(AP_NavEKF_Source::SourceXY::GPS, core_index)) {
846 199774 : return;
847 : }
848 :
849 19757410 : if (EKFGSF_run_filterbank) {
850 13887176 : if (gpsDataToFuse) {
851 1007751 : Vector2F gpsVelNE = Vector2F(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
852 1007751 : ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise));
853 1007751 : yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc);
854 :
855 : // after velocity data has been fused the yaw variance estimate will have been refreshed and
856 : // is used maintain a history of validity
857 : ftype gsfYaw, gsfYawVariance;
858 1007751 : if (EKFGSF_getYaw(gsfYaw, gsfYawVariance)) {
859 655893 : if (EKFGSF_yaw_valid_count < GSF_YAW_VALID_HISTORY_THRESHOLD) {
860 11795 : EKFGSF_yaw_valid_count++;
861 : }
862 : } else {
863 351858 : EKFGSF_yaw_valid_count = 0;
864 : }
865 : }
866 :
867 : // action an external reset request
868 13887176 : if (EKFGSF_yaw_reset_request_ms > 0 && imuSampleTime_ms - EKFGSF_yaw_reset_request_ms < YAW_RESET_TO_GSF_TIMEOUT_MS) {
869 1616 : EKFGSF_resetMainFilterYaw(true);
870 : }
871 : } else {
872 5870234 : EKFGSF_yaw_valid_count = 0;
873 : }
874 : }
875 :
876 : // request a reset the yaw to the GSF estimate
877 : // request times out after YAW_RESET_TO_GSF_TIMEOUT_MS if it cannot be actioned
878 8 : void NavEKF3_core::EKFGSF_requestYawReset()
879 : {
880 8 : EKFGSF_yaw_reset_request_ms = imuSampleTime_ms;
881 8 : }
|
