Line data Source code
1 : #include "AP_NavEKF2_core.h"
2 :
3 : #include <GCS_MAVLink/GCS.h>
4 : #include <AP_DAL/AP_DAL.h>
5 :
6 : #include "AP_NavEKF2.h"
7 :
8 : // Control filter mode transitions
9 4213784 : void NavEKF2_core::controlFilterModes()
10 : {
11 : // Determine motor arm status
12 4213784 : prevMotorsArmed = motorsArmed;
13 4213784 : motorsArmed = dal.get_armed();
14 4213784 : if (motorsArmed && !prevMotorsArmed) {
15 : // set the time at which we arm to assist with checks
16 388 : timeAtArming_ms = imuSampleTime_ms;
17 : }
18 :
19 : // Detect if we are in flight on or ground
20 4213784 : detectFlight();
21 :
22 : // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
23 : // avoid unnecessary operations
24 4213784 : setWindMagStateLearningMode();
25 :
26 : // Check the alignmnent status of the tilt and yaw attitude
27 : // Used during initial bootstrap alignment of the filter
28 4213784 : checkAttitudeAlignmentStatus();
29 :
30 : // Set the type of inertial navigation aiding used
31 4213784 : setAidingMode();
32 :
33 4213784 : }
34 :
35 : /*
36 : return effective value for _magCal for this core
37 : */
38 4213784 : uint8_t NavEKF2_core::effective_magCal(void) const
39 : {
40 : // force use of simple magnetic heading fusion for specified cores
41 4213784 : if (frontend->_magMask & core_index) {
42 : return 2;
43 : } else {
44 4213784 : return frontend->_magCal;
45 : }
46 : }
47 :
48 : // Determine if learning of wind and magnetic field will be enabled and set corresponding indexing limits to
49 : // avoid unnecessary operations
50 4213784 : void NavEKF2_core::setWindMagStateLearningMode()
51 : {
52 : // 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
53 4213784 : bool setWindInhibit = (!useAirspeed() && !assume_zero_sideslip()) || onGround || (PV_AidingMode == AID_NONE);
54 4213784 : if (!inhibitWindStates && setWindInhibit) {
55 262 : inhibitWindStates = true;
56 4213522 : } else if (inhibitWindStates && !setWindInhibit) {
57 284 : inhibitWindStates = false;
58 : // set states and variances
59 284 : if (yawAlignComplete && useAirspeed()) {
60 : // if we have airspeed and a valid heading, set the wind states to the reciprocal of the vehicle heading
61 : // which assumes the vehicle has launched into the wind
62 266 : Vector3F tempEuler;
63 266 : stateStruct.quat.to_euler(tempEuler.x, tempEuler.y, tempEuler.z);
64 266 : ftype windSpeed = sqrtF(sq(stateStruct.velocity.x) + sq(stateStruct.velocity.y)) - tasDataDelayed.tas;
65 266 : stateStruct.wind_vel.x = windSpeed * cosF(tempEuler.z);
66 266 : stateStruct.wind_vel.y = windSpeed * sinF(tempEuler.z);
67 :
68 : // set the wind sate variances to the measurement uncertainty
69 798 : for (uint8_t index=22; index<=23; index++) {
70 532 : P[index][index] = sq(constrain_ftype(frontend->_easNoise, 0.5f, 5.0f) * constrain_ftype(dal.get_EAS2TAS(), 0.9f, 10.0f));
71 : }
72 : } else {
73 : // set the variances using a typical wind speed
74 54 : for (uint8_t index=22; index<=23; index++) {
75 36 : P[index][index] = sq(5.0f);
76 : }
77 : }
78 : }
79 :
80 : // determine if the vehicle is manoeuvring
81 4213784 : if (accNavMagHoriz > 0.5f) {
82 1727854 : manoeuvring = true;
83 : } else {
84 2485930 : manoeuvring = false;
85 : }
86 :
87 : // Determine if learning of magnetic field states has been requested by the user
88 4213784 : uint8_t magCal = effective_magCal();
89 : bool magCalRequested =
90 4213784 : ((magCal == 0) && inFlight) || // when flying
91 1893630 : ((magCal == 1) && manoeuvring) || // when manoeuvring
92 6107414 : ((magCal == 3) && finalInflightYawInit && finalInflightMagInit) || // when initial in-air yaw and mag field reset is complete
93 : (magCal == 4); // all the time
94 :
95 : // Deny mag calibration request if we aren't using the compass, it has been inhibited by the user,
96 : // we do not have an absolute position reference or are on the ground (unless explicitly requested by the user)
97 4213784 : bool magCalDenied = !use_compass() || (magCal == 2) || (onGround && magCal != 4);
98 :
99 : // Inhibit the magnetic field calibration if not requested or denied
100 4213784 : bool setMagInhibit = !magCalRequested || magCalDenied;
101 4213784 : if (!inhibitMagStates && setMagInhibit) {
102 282 : inhibitMagStates = true;
103 : // variances will be reset in CovariancePrediction
104 4213502 : } else if (inhibitMagStates && !setMagInhibit) {
105 308 : inhibitMagStates = false;
106 308 : if (magFieldLearned) {
107 : // if we have already learned the field states, then retain the learned variances
108 2 : P[16][16] = earthMagFieldVar.x;
109 2 : P[17][17] = earthMagFieldVar.y;
110 2 : P[18][18] = earthMagFieldVar.z;
111 2 : P[19][19] = bodyMagFieldVar.x;
112 2 : P[20][20] = bodyMagFieldVar.y;
113 2 : P[21][21] = bodyMagFieldVar.z;
114 : } else {
115 : // set the variances equal to the observation variances
116 2142 : for (uint8_t index=16; index<=21; index++) {
117 1836 : P[index][index] = sq(frontend->_magNoise);
118 : }
119 :
120 : // set the NE earth magnetic field states using the published declination
121 : // and set the corresponding variances and covariances
122 306 : alignMagStateDeclination();
123 :
124 : }
125 : // request a reset of the yaw and magnetic field states if not done before
126 308 : if (!magStateInitComplete || (!finalInflightMagInit && inFlight)) {
127 290 : magYawResetRequest = true;
128 : }
129 : }
130 :
131 : // If on ground we clear the flag indicating that the magnetic field in-flight initialisation has been completed
132 : // because we want it re-done for each takeoff
133 4213784 : if (onGround) {
134 1220526 : finalInflightYawInit = false;
135 1220526 : finalInflightMagInit = false;
136 : }
137 :
138 : // Adjust the indexing limits used to address the covariance, states and other EKF arrays to avoid unnecessary operations
139 : // if we are not using those states
140 4213784 : if (inhibitMagStates && inhibitWindStates) {
141 1391332 : stateIndexLim = 15;
142 2822452 : } else if (inhibitWindStates) {
143 638319 : stateIndexLim = 21;
144 : } else {
145 2184133 : stateIndexLim = 23;
146 : }
147 4213784 : }
148 :
149 : // Set inertial navigation aiding mode
150 4213784 : void NavEKF2_core::setAidingMode()
151 : {
152 : // Save the previous status so we can detect when it has changed
153 4213784 : PV_AidingModePrev = PV_AidingMode;
154 :
155 : // Determine if we should change aiding mode
156 4213784 : switch (PV_AidingMode) {
157 888463 : case AID_NONE: {
158 : // Don't allow filter to start position or velocity aiding until the tilt and yaw alignment is complete
159 : // and IMU gyro bias estimates have stabilised
160 888463 : bool filterIsStable = tiltAlignComplete && yawAlignComplete && checkGyroCalStatus();
161 : // If GPS usage has been prohiited then we use flow aiding provided optical flow data is present
162 : // GPS aiding is the preferred option unless excluded by the user
163 888463 : bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS() && filterIsStable);
164 888463 : bool canUseRangeBeacon = readyToUseRangeBeacon() && filterIsStable;
165 888463 : bool canUseExtNav = readyToUseExtNav();
166 888463 : if(canUseGPS || canUseRangeBeacon || canUseExtNav) {
167 250 : PV_AidingMode = AID_ABSOLUTE;
168 888213 : } else if (optFlowDataPresent() && (frontend->_flowUse == FLOW_USE_NAV) && filterIsStable) {
169 0 : PV_AidingMode = AID_RELATIVE;
170 : }
171 : }
172 : break;
173 :
174 0 : case AID_RELATIVE: {
175 : // Check if the optical flow sensor has timed out
176 0 : bool flowSensorTimeout = ((imuSampleTime_ms - flowValidMeaTime_ms) > 5000);
177 : // Check if the fusion has timed out (flow measurements have been rejected for too long)
178 0 : bool flowFusionTimeout = ((imuSampleTime_ms - prevFlowFuseTime_ms) > 5000);
179 : // Enable switch to absolute position mode if GPS is available
180 : // If GPS is not available and flow fusion has timed out, then fall-back to no-aiding
181 0 : if((frontend->_fusionModeGPS) != 3 && readyToUseGPS()) {
182 0 : PV_AidingMode = AID_ABSOLUTE;
183 0 : } else if (flowSensorTimeout || flowFusionTimeout) {
184 0 : PV_AidingMode = AID_NONE;
185 : }
186 : }
187 : break;
188 :
189 3325321 : case AID_ABSOLUTE: {
190 : // Find the minimum time without data required to trigger any check
191 3325321 : uint16_t minTestTime_ms = MIN(frontend->tiltDriftTimeMax_ms, MIN(frontend->posRetryTimeNoVel_ms,frontend->posRetryTimeUseVel_ms));
192 :
193 : // Check if optical flow data is being used
194 3325321 : bool optFlowUsed = (imuSampleTime_ms - prevFlowFuseTime_ms <= minTestTime_ms);
195 :
196 : // Check if airspeed data is being used
197 3325321 : bool airSpdUsed = (imuSampleTime_ms - lastTasPassTime_ms <= minTestTime_ms);
198 :
199 : // Check if range beacon data is being used
200 3325321 : bool rngBcnUsed = (imuSampleTime_ms - lastRngBcnPassTime_ms <= minTestTime_ms);
201 :
202 : // Check if GPS is being used
203 3325321 : bool posUsed = (imuSampleTime_ms - lastPosPassTime_ms <= minTestTime_ms);
204 3325321 : bool gpsVelUsed = (imuSampleTime_ms - lastVelPassTime_ms <= minTestTime_ms);
205 :
206 : // Check if external nav is being used
207 3325321 : bool extNavUsed = (imuSampleTime_ms - lastExtNavPassTime_ms <= minTestTime_ms);
208 :
209 : // Check if attitude drift has been constrained by a measurement source
210 3325321 : bool attAiding = posUsed || gpsVelUsed || optFlowUsed || airSpdUsed || rngBcnUsed || extNavUsed;
211 :
212 : // check if velocity drift has been constrained by a measurement source
213 3325321 : bool velAiding = gpsVelUsed || airSpdUsed || optFlowUsed;
214 :
215 : // check if position drift has been constrained by a measurement source
216 3325321 : bool posAiding = posUsed || rngBcnUsed || extNavUsed;
217 :
218 : // Check if the loss of attitude aiding has become critical
219 : bool attAidLossCritical = false;
220 3325321 : if (!attAiding) {
221 1848 : attAidLossCritical = (imuSampleTime_ms - prevFlowFuseTime_ms > frontend->tiltDriftTimeMax_ms) &&
222 1846 : (imuSampleTime_ms - lastTasPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
223 1846 : (imuSampleTime_ms - lastRngBcnPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
224 2 : (imuSampleTime_ms - lastPosPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
225 1850 : (imuSampleTime_ms - lastExtNavPassTime_ms > frontend->tiltDriftTimeMax_ms) &&
226 : (imuSampleTime_ms - lastVelPassTime_ms > frontend->tiltDriftTimeMax_ms);
227 : }
228 :
229 : // Check if the loss of position accuracy has become critical
230 : bool posAidLossCritical = false;
231 3325321 : if (!posAiding ) {
232 : uint16_t maxLossTime_ms;
233 24032 : if (!velAiding) {
234 1848 : maxLossTime_ms = frontend->posRetryTimeNoVel_ms;
235 : } else {
236 22184 : maxLossTime_ms = frontend->posRetryTimeUseVel_ms;
237 : }
238 22108 : posAidLossCritical = (imuSampleTime_ms - lastRngBcnPassTime_ms > maxLossTime_ms) &&
239 46140 : (imuSampleTime_ms - lastExtNavPassTime_ms > maxLossTime_ms) &&
240 : (imuSampleTime_ms - lastPosPassTime_ms > maxLossTime_ms);
241 : }
242 :
243 3325321 : if (attAidLossCritical) {
244 : // if the loss of attitude data is critical, then put the filter into a constant position mode
245 2 : PV_AidingMode = AID_NONE;
246 2 : posTimeout = true;
247 2 : velTimeout = true;
248 2 : rngBcnTimeout = true;
249 2 : tasTimeout = true;
250 2 : gpsNotAvailable = true;
251 3325319 : } else if (posAidLossCritical) {
252 14534 : if ((frontend->_flowUse == FLOW_USE_NAV) && optFlowDataPresent() && (imuSampleTime_ms - rngValidMeaTime_ms < 500)) {
253 0 : PV_AidingMode = AID_NONE;
254 : }
255 : // if the loss of position is critical, declare all sources of position aiding as being timed out
256 14534 : posTimeout = true;
257 14534 : velTimeout = true;
258 14534 : rngBcnTimeout = true;
259 14534 : gpsNotAvailable = true;
260 : }
261 : break;
262 : }
263 : }
264 :
265 : // check to see if we are starting or stopping aiding and set states and modes as required
266 4213784 : if (PV_AidingMode != PV_AidingModePrev) {
267 : // set various usage modes based on the condition when we start aiding. These are then held until aiding is stopped.
268 252 : switch (PV_AidingMode) {
269 2 : case AID_NONE:
270 : // We have ceased aiding
271 2 : GCS_SEND_TEXT(MAV_SEVERITY_WARNING, "EKF2 IMU%u has stopped aiding",(unsigned)imu_index);
272 : // When not aiding, estimate orientation & height fusing synthetic constant position and zero velocity measurement to constrain tilt errors
273 2 : posTimeout = true;
274 2 : velTimeout = true;
275 : // Reset the normalised innovation to avoid false failing bad fusion tests
276 2 : velTestRatio = 0.0f;
277 2 : posTestRatio = 0.0f;
278 : // store the current position to be used to keep reporting the last known position
279 2 : lastKnownPositionNE.x = stateStruct.position.x;
280 2 : lastKnownPositionNE.y = stateStruct.position.y;
281 : // initialise filtered altitude used to provide a takeoff reference to current baro on disarm
282 : // this reduces the time required for the baro noise filter to settle before the filtered baro data can be used
283 2 : meaHgtAtTakeOff = baroDataDelayed.hgt;
284 : // reset the vertical position state to faster recover from baro errors experienced during touchdown
285 2 : stateStruct.position.z = -meaHgtAtTakeOff;
286 2 : break;
287 :
288 0 : case AID_RELATIVE:
289 : // We have commenced aiding, but GPS usage has been prohibited so use optical flow only
290 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u is using optical flow",(unsigned)imu_index);
291 0 : posTimeout = true;
292 0 : velTimeout = true;
293 : // Reset the last valid flow measurement time
294 0 : flowValidMeaTime_ms = imuSampleTime_ms;
295 : // Reset the last valid flow fusion time
296 0 : prevFlowFuseTime_ms = imuSampleTime_ms;
297 0 : break;
298 :
299 250 : case AID_ABSOLUTE: {
300 250 : bool canUseGPS = ((frontend->_fusionModeGPS) != 3 && readyToUseGPS());
301 250 : bool canUseRangeBeacon = readyToUseRangeBeacon();
302 250 : bool canUseExtNav = readyToUseExtNav();
303 : // We have commenced aiding and GPS usage is allowed
304 250 : if (canUseGPS) {
305 250 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u is using GPS",(unsigned)imu_index);
306 : }
307 250 : posTimeout = false;
308 250 : velTimeout = false;
309 : // We have commenced aiding and range beacon usage is allowed
310 250 : if (canUseRangeBeacon) {
311 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u is using range beacons",(unsigned)imu_index);
312 0 : 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);
313 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u initial beacon pos D offset = %3.1f (m)",(unsigned)imu_index,(double)bcnPosOffset);
314 : }
315 : // We have commenced aiding and external nav usage is allowed
316 250 : if (canUseExtNav) {
317 0 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u is using external nav data",(unsigned)imu_index);
318 0 : 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);
319 : //handle yaw reset as special case only if compass is disabled
320 0 : if (!use_compass()) {
321 0 : extNavYawResetRequest = true;
322 0 : controlMagYawReset();
323 : }
324 : // handle height reset as special case
325 0 : hgtMea = -extNavDataDelayed.pos.z;
326 0 : posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.1f, 10.0f));
327 0 : ResetHeight();
328 : }
329 : // reset the last fusion accepted times to prevent unwanted activation of timeout logic
330 250 : lastPosPassTime_ms = imuSampleTime_ms;
331 250 : lastVelPassTime_ms = imuSampleTime_ms;
332 250 : lastRngBcnPassTime_ms = imuSampleTime_ms;
333 250 : lastExtNavPassTime_ms = imuSampleTime_ms;
334 : }
335 250 : break;
336 : }
337 :
338 : // Always reset the position and velocity when changing mode
339 252 : ResetVelocity();
340 252 : ResetPosition();
341 : }
342 4213784 : }
343 :
344 : // Check the tilt and yaw alignmnent status
345 : // Used during initial bootstrap alignment of the filter
346 4213784 : void NavEKF2_core::checkAttitudeAlignmentStatus()
347 : {
348 : // Check for tilt convergence - used during initial alignment
349 4213784 : ftype alpha = 1.0f*imuDataDelayed.delAngDT;
350 4213784 : ftype temp=tiltErrVec.length();
351 4213784 : tiltErrFilt = alpha*temp + (1.0f-alpha)*tiltErrFilt;
352 4213784 : if (tiltErrFilt < 0.005f && !tiltAlignComplete) {
353 418 : tiltAlignComplete = true;
354 418 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u tilt alignment complete",(unsigned)imu_index);
355 : }
356 :
357 : // submit yaw and magnetic field reset requests depending on whether we have compass data
358 4213784 : if (tiltAlignComplete && !yawAlignComplete) {
359 760 : if (use_compass()) {
360 0 : magYawResetRequest = true;
361 0 : gpsYawResetRequest = false;
362 : } else {
363 760 : magYawResetRequest = false;
364 760 : gpsYawResetRequest = true;
365 : }
366 : }
367 4213784 : }
368 :
369 : // return true if we should use the airspeed sensor
370 7405944 : bool NavEKF2_core::useAirspeed(void) const
371 : {
372 7405944 : return dal.airspeed_sensor_enabled();
373 : }
374 :
375 : // return true if we should use the range finder sensor
376 0 : bool NavEKF2_core::useRngFinder(void) const
377 : {
378 : // TO-DO add code to set this based in setting of optical flow use parameter and presence of sensor
379 0 : return true;
380 : }
381 :
382 : // return true if optical flow data is available
383 888853 : bool NavEKF2_core::optFlowDataPresent(void) const
384 : {
385 888853 : return (imuSampleTime_ms - flowMeaTime_ms < 200);
386 : }
387 :
388 : // return true if the filter to be ready to use gps
389 888713 : bool NavEKF2_core::readyToUseGPS(void) const
390 : {
391 888713 : return validOrigin && tiltAlignComplete && yawAlignComplete && gpsGoodToAlign && (frontend->_fusionModeGPS != 3) && gpsDataToFuse;
392 : }
393 :
394 : // return true if the filter to be ready to use the beacon range measurements
395 888713 : bool NavEKF2_core::readyToUseRangeBeacon(void) const
396 : {
397 888713 : return tiltAlignComplete && yawAlignComplete && rngBcnGoodToAlign && rngBcnDataToFuse;
398 : }
399 :
400 : // return true if the filter to be ready to use external nav data
401 888713 : bool NavEKF2_core::readyToUseExtNav(void) const
402 : {
403 888713 : return tiltAlignComplete && extNavDataToFuse;
404 : }
405 :
406 : // return true if we should use the compass
407 16320620 : bool NavEKF2_core::use_compass(void) const
408 : {
409 16320620 : return dal.compass().use_for_yaw(magSelectIndex) && !allMagSensorsFailed;
410 : }
411 :
412 : /*
413 : should we assume zero sideslip?
414 : */
415 11869049 : bool NavEKF2_core::assume_zero_sideslip(void) const
416 : {
417 : // we don't assume zero sideslip for ground vehicles as EKF could
418 : // be quite sensitive to a rapid spin of the ground vehicle if
419 : // traction is lost
420 11869049 : return dal.get_fly_forward() && dal.get_vehicle_class() != AP_DAL::VehicleClass::GROUND;
421 : }
422 :
423 : // sets the local NED origin using a LLH location (latitude, longitude, height)
424 : // returns false if absolute aiding and GPS is being used or if the origin is already set
425 34 : bool NavEKF2_core::setOriginLLH(const Location &loc)
426 : {
427 34 : if (PV_AidingMode == AID_ABSOLUTE && !extNavUsedForPos) {
428 : // reject attempts to set the origin if GPS is being used
429 : return false;
430 : }
431 :
432 34 : return setOrigin(loc);
433 : }
434 :
435 : // sets the local NED origin using a LLH location (latitude, longitude, height)
436 : // returns false if the origin has already been set
437 394 : bool NavEKF2_core::setOrigin(const Location &loc)
438 : {
439 : // if the origin is valid reject setting a new origin
440 394 : if (validOrigin) {
441 : return false;
442 : }
443 :
444 394 : EKF_origin = loc;
445 394 : ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
446 : // define Earth rotation vector in the NED navigation frame at the origin
447 394 : calcEarthRateNED(earthRateNED, EKF_origin.lat);
448 394 : validOrigin = true;
449 394 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u origin set",(unsigned)imu_index);
450 :
451 : // put origin in frontend as well to ensure it stays in sync between lanes
452 394 : frontend->common_EKF_origin = EKF_origin;
453 394 : frontend->common_origin_valid = true;
454 :
455 394 : return true;
456 : }
457 :
458 : // record a yaw reset event
459 846 : void NavEKF2_core::recordYawReset()
460 : {
461 846 : yawAlignComplete = true;
462 846 : if (inFlight) {
463 38 : finalInflightYawInit = true;
464 : }
465 846 : }
466 :
467 : // return true and set the class variable true if the delta angle bias has been learned
468 747519 : bool NavEKF2_core::checkGyroCalStatus(void)
469 : {
470 : // check delta angle bias variances
471 747519 : const ftype delAngBiasVarMax = sq(radians(0.15f * dtEkfAvg));
472 747519 : if (!use_compass()) {
473 : // rotate the variances into earth frame and evaluate horizontal terms only as yaw component is poorly observable without a compass
474 : // which can make this check fail
475 8 : Vector3F delAngBiasVarVec = Vector3F(P[9][9],P[10][10],P[11][11]);
476 8 : Vector3F temp = prevTnb * delAngBiasVarVec;
477 16 : delAngBiasLearned = (fabsF(temp.x) < delAngBiasVarMax) &&
478 8 : (fabsF(temp.y) < delAngBiasVarMax);
479 : } else {
480 747511 : delAngBiasLearned = (P[9][9] <= delAngBiasVarMax) &&
481 747511 : (P[10][10] <= delAngBiasVarMax) &&
482 537294 : (P[11][11] <= delAngBiasVarMax);
483 : }
484 747519 : return delAngBiasLearned;
485 : }
486 :
487 : // Update the filter status
488 3248540 : void NavEKF2_core::updateFilterStatus(void)
489 : {
490 : // init return value
491 3248540 : filterStatus.value = 0;
492 3248540 : bool doingFlowNav = (PV_AidingMode == AID_RELATIVE) && flowDataValid;
493 3248540 : bool doingWindRelNav = !tasTimeout && assume_zero_sideslip();
494 3248540 : bool doingNormalGpsNav = !posTimeout && (PV_AidingMode == AID_ABSOLUTE);
495 3248540 : bool someVertRefData = (!velTimeout && (useGpsVertVel || useExtNavVel)) || !hgtTimeout;
496 3248540 : bool someHorizRefData = !(velTimeout && posTimeout && tasTimeout) || doingFlowNav;
497 3248540 : bool optFlowNavPossible = flowDataValid && delAngBiasLearned;
498 3248540 : bool gpsNavPossible = !gpsNotAvailable && gpsGoodToAlign && delAngBiasLearned;
499 3248540 : bool filterHealthy = healthy() && tiltAlignComplete && (yawAlignComplete || (!use_compass() && (PV_AidingMode == AID_NONE)));
500 : // If GPS height usage is specified, height is considered to be inaccurate until the GPS passes all checks
501 3248540 : bool hgtNotAccurate = (frontend->_altSource == 2) && !validOrigin;
502 :
503 : // set individual flags
504 3248540 : filterStatus.flags.attitude = !stateStruct.quat.is_nan() && filterHealthy; // attitude valid (we need a better check)
505 3248540 : filterStatus.flags.horiz_vel = someHorizRefData && filterHealthy; // horizontal velocity estimate valid
506 3248540 : filterStatus.flags.vert_vel = someVertRefData && filterHealthy; // vertical velocity estimate valid
507 3248540 : filterStatus.flags.horiz_pos_rel = ((doingFlowNav && gndOffsetValid) || doingWindRelNav || doingNormalGpsNav) && filterHealthy; // relative horizontal position estimate valid
508 3248540 : filterStatus.flags.horiz_pos_abs = doingNormalGpsNav && filterHealthy; // absolute horizontal position estimate valid
509 3248540 : filterStatus.flags.vert_pos = !hgtTimeout && filterHealthy && !hgtNotAccurate; // vertical position estimate valid
510 3248540 : filterStatus.flags.terrain_alt = gndOffsetValid && filterHealthy; // terrain height estimate valid
511 3248540 : filterStatus.flags.const_pos_mode = (PV_AidingMode == AID_NONE) && filterHealthy; // constant position mode
512 3248540 : 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
513 3248540 : 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
514 3248540 : filterStatus.flags.takeoff_detected = takeOffDetected; // takeoff for optical flow navigation has been detected
515 3248540 : filterStatus.flags.takeoff = dal.get_takeoff_expected(); // The EKF has been told to expect takeoff and is in a ground effect mitigation mode
516 3248540 : filterStatus.flags.touchdown = dal.get_touchdown_expected(); // The EKF has been told to detect touchdown and is in a ground effect mitigation mode
517 3248540 : filterStatus.flags.using_gps = ((imuSampleTime_ms - lastPosPassTime_ms) < 4000) && (PV_AidingMode == AID_ABSOLUTE);
518 3248540 : filterStatus.flags.gps_glitching = !gpsAccuracyGood && (PV_AidingMode == AID_ABSOLUTE) && !extNavUsedForPos; // GPS glitching is affecting navigation accuracy
519 3248540 : filterStatus.flags.gps_quality_good = gpsGoodToAlign;
520 : // for reporting purposes we report rejecting airspeed after 3s of not fusing when we want to fuse the data
521 6508474 : filterStatus.flags.rejecting_airspeed = lastTasFailTime_ms != 0 &&
522 3248540 : (imuSampleTime_ms - lastTasFailTime_ms) < 1000 &&
523 6110 : (imuSampleTime_ms - lastTasPassTime_ms) > 3000;
524 3248540 : filterStatus.flags.initalized = filterStatus.flags.initalized || healthy();
525 3248540 : }
526 :
527 3248540 : void NavEKF2_core::runYawEstimatorPrediction()
528 : {
529 3248540 : if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1) {
530 : ftype trueAirspeed;
531 3248540 : if (is_positive(defaultAirSpeed) && assume_zero_sideslip()) {
532 0 : if (imuDataDelayed.time_ms - tasDataDelayed.time_ms < 5000) {
533 0 : trueAirspeed = tasDataDelayed.tas;
534 : } else {
535 0 : trueAirspeed = defaultAirSpeed * dal.get_EAS2TAS();
536 : }
537 : } else {
538 : trueAirspeed = 0.0f;
539 : }
540 :
541 3248540 : yawEstimator->update(imuDataDelayed.delAng, imuDataDelayed.delVel, imuDataDelayed.delAngDT, imuDataDelayed.delVelDT, EKFGSF_run_filterbank, trueAirspeed);
542 : }
543 3248540 : }
544 :
545 3248540 : void NavEKF2_core::runYawEstimatorCorrection()
546 : {
547 3248540 : if (yawEstimator != nullptr && frontend->_fusionModeGPS <= 1 && EKFGSF_run_filterbank) {
548 2340576 : if (gpsDataToFuse) {
549 224936 : Vector2F gpsVelNE = Vector2F(gpsDataDelayed.vel.x, gpsDataDelayed.vel.y);
550 224936 : ftype gpsVelAcc = fmaxF(gpsSpdAccuracy, ftype(frontend->_gpsHorizVelNoise));
551 224936 : yawEstimator->fuseVelData(gpsVelNE, gpsVelAcc);
552 : }
553 :
554 : // action an external reset request
555 2340576 : if (EKFGSF_yaw_reset_request_ms > 0 && imuSampleTime_ms - EKFGSF_yaw_reset_request_ms < YAW_RESET_TO_GSF_TIMEOUT_MS) {
556 4 : EKFGSF_resetMainFilterYaw();
557 : }
558 : }
559 3248540 : }
560 :
561 : // request a reset the yaw to the GSF estimate
562 : // request times out after YAW_RESET_TO_GSF_TIMEOUT_MS if it cannot be actioned
563 4 : void NavEKF2_core::EKFGSF_requestYawReset()
564 : {
565 4 : EKFGSF_yaw_reset_request_ms = imuSampleTime_ms;
566 4 : }
|
