Line data Source code
1 : #include "AP_NavEKF2_core.h"
2 :
3 : #include "AP_NavEKF2.h"
4 :
5 : #include <AP_DAL/AP_DAL.h>
6 :
7 : /* Monitor GPS data to see if quality is good enough to initialise the EKF
8 : Monitor magnetometer innovations to see if the heading is good enough to use GPS
9 : Return true if all criteria pass for 10 seconds
10 :
11 : We also record the failure reason so that pre_arm_check()
12 : can give a good report to the user on why arming is failing
13 :
14 : This sets gpsGoodToAlign class variable
15 : */
16 312142 : void NavEKF2_core::calcGpsGoodToAlign(void)
17 : {
18 312142 : const auto &gps = dal.gps();
19 :
20 312142 : if (inFlight && assume_zero_sideslip() && !use_compass()) {
21 : // this is a special case where a plane has launched without magnetometer
22 : // is now in the air and needs to align yaw to the GPS and start navigating as soon as possible
23 1496 : gpsGoodToAlign = true;
24 1496 : return;
25 : }
26 :
27 : // User defined multiplier to be applied to check thresholds
28 310646 : ftype checkScaler = 0.01f*(ftype)frontend->_gpsCheckScaler;
29 :
30 310646 : if (gpsGoodToAlign) {
31 : /*
32 : if we have already passed GPS alignment checks then raise
33 : the check threshold so that we have some hysterisis and
34 : don't continuously change from able to arm to not able to
35 : arm
36 : */
37 283378 : checkScaler *= 1.3f;
38 : }
39 :
40 : // If we have good magnetometer consistency and bad innovations for longer than 5 seconds then we reset heading and field states
41 : // This enables us to handle large changes to the external magnetic field environment that occur before arming
42 310646 : if ((magTestRatio.x <= 1.0f && magTestRatio.y <= 1.0f && yawTestRatio <= 1.0f) || !consistentMagData) {
43 308614 : magYawResetTimer_ms = imuSampleTime_ms;
44 : }
45 310646 : if ((imuSampleTime_ms - magYawResetTimer_ms > 5000) && !motorsArmed) {
46 : // request reset of heading and magnetic field states
47 8 : magYawResetRequest = true;
48 : // reset timer to ensure that bad magnetometer data cannot cause a heading reset more often than every 5 seconds
49 8 : magYawResetTimer_ms = imuSampleTime_ms;
50 : }
51 :
52 : // Check for significant change in GPS position if disarmed which indicates bad GPS
53 : // This check can only be used when the vehicle is stationary
54 310646 : const Location &gpsloc = gps.location(); // Current location
55 : const ftype posFiltTimeConst = 10.0f; // time constant used to decay position drift
56 : // calculate time lapsed since last update and limit to prevent numerical errors
57 310646 : ftype deltaTime = constrain_ftype(float(imuDataDelayed.time_ms - lastPreAlignGpsCheckTime_ms)*0.001f,0.01f,posFiltTimeConst);
58 310646 : lastPreAlignGpsCheckTime_ms = imuDataDelayed.time_ms;
59 : // Sum distance moved
60 310646 : gpsDriftNE += gpsloc_prev.get_distance(gpsloc);
61 310646 : gpsloc_prev = gpsloc;
62 : // Decay distance moved exponentially to zero
63 310646 : gpsDriftNE *= (1.0f - deltaTime/posFiltTimeConst);
64 : // Clamp the filter state to prevent excessive persistence of large transients
65 310646 : gpsDriftNE = MIN(gpsDriftNE,10.0f);
66 : // Fail if more than 3 metres drift after filtering whilst on-ground
67 : // This corresponds to a maximum acceptable average drift rate of 0.3 m/s or single glitch event of 3m
68 310646 : bool gpsDriftFail = (gpsDriftNE > 3.0f*checkScaler) && onGround && (frontend->_gpsCheck & MASK_GPS_POS_DRIFT);
69 :
70 : // Report check result as a text string and bitmask
71 : if (gpsDriftFail) {
72 0 : dal.snprintf(prearm_fail_string,
73 : sizeof(prearm_fail_string),
74 0 : "GPS drift %.1fm (needs %.1f)", (double)gpsDriftNE, (double)(3.0f*checkScaler));
75 0 : gpsCheckStatus.bad_horiz_drift = true;
76 : } else {
77 310646 : gpsCheckStatus.bad_horiz_drift = false;
78 : }
79 :
80 : // Check that the vertical GPS vertical velocity is reasonable after noise filtering
81 : bool gpsVertVelFail;
82 310646 : if (gps.have_vertical_velocity() && onGround) {
83 : // check that the average vertical GPS velocity is close to zero
84 85148 : gpsVertVelFilt = 0.1f * gpsDataNew.vel.z + 0.9f * gpsVertVelFilt;
85 85148 : gpsVertVelFilt = constrain_ftype(gpsVertVelFilt,-10.0f,10.0f);
86 85148 : gpsVertVelFail = (fabsF(gpsVertVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_VERT_SPD);
87 : } else {
88 : gpsVertVelFail = false;
89 : }
90 :
91 : // Report check result as a text string and bitmask
92 : if (gpsVertVelFail) {
93 0 : dal.snprintf(prearm_fail_string,
94 : sizeof(prearm_fail_string),
95 0 : "GPS vertical speed %.2fm/s (needs %.2f)", (double)fabsF(gpsVertVelFilt), (double)(0.3f*checkScaler));
96 0 : gpsCheckStatus.bad_vert_vel = true;
97 : } else {
98 310646 : gpsCheckStatus.bad_vert_vel = false;
99 : }
100 :
101 : // Check that the horizontal GPS vertical velocity is reasonable after noise filtering
102 : // This check can only be used if the vehicle is stationary
103 : bool gpsHorizVelFail;
104 310646 : if (onGround) {
105 85148 : gpsHorizVelFilt = 0.1f * gpsDataDelayed.vel.xy().length() + 0.9f * gpsHorizVelFilt;
106 85148 : gpsHorizVelFilt = constrain_ftype(gpsHorizVelFilt,-10.0f,10.0f);
107 85148 : gpsHorizVelFail = (fabsF(gpsHorizVelFilt) > 0.3f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_HORIZ_SPD);
108 : } else {
109 : gpsHorizVelFail = false;
110 : }
111 :
112 : // Report check result as a text string and bitmask
113 : if (gpsHorizVelFail) {
114 0 : dal.snprintf(prearm_fail_string,
115 : sizeof(prearm_fail_string),
116 0 : "GPS horizontal speed %.2fm/s (needs %.2f)", (double)gpsDriftNE, (double)(0.3f*checkScaler));
117 0 : gpsCheckStatus.bad_horiz_vel = true;
118 : } else {
119 310646 : gpsCheckStatus.bad_horiz_vel = false;
120 : }
121 :
122 : // fail if horiziontal position accuracy not sufficient
123 310646 : float hAcc = 0.0;
124 : bool hAccFail;
125 310646 : if (gps.horizontal_accuracy(hAcc)) {
126 310646 : hAccFail = (hAcc > 5.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
127 : } else {
128 : hAccFail = false;
129 : }
130 :
131 : // Report check result as a text string and bitmask
132 : if (hAccFail) {
133 326 : dal.snprintf(prearm_fail_string,
134 : sizeof(prearm_fail_string),
135 : "GPS horiz error %.1fm (needs %.1f)", (double)hAcc, (double)(5.0f*checkScaler));
136 326 : gpsCheckStatus.bad_hAcc = true;
137 : } else {
138 310320 : gpsCheckStatus.bad_hAcc = false;
139 : }
140 :
141 : // Check for vertical GPS accuracy
142 310646 : float vAcc = 0.0f;
143 : bool vAccFail = false;
144 310646 : if (gps.vertical_accuracy(vAcc)) {
145 310646 : vAccFail = (vAcc > 7.5f * checkScaler) && (frontend->_gpsCheck & MASK_GPS_POS_ERR);
146 : }
147 : // Report check result as a text string and bitmask
148 : if (vAccFail) {
149 326 : dal.snprintf(prearm_fail_string,
150 : sizeof(prearm_fail_string),
151 : "GPS vert error %.1fm (needs < %.1f)", (double)vAcc, (double)(7.5f * checkScaler));
152 326 : gpsCheckStatus.bad_vAcc = true;
153 : } else {
154 310320 : gpsCheckStatus.bad_vAcc = false;
155 : }
156 :
157 : // fail if reported speed accuracy greater than threshold
158 310646 : bool gpsSpdAccFail = (gpsSpdAccuracy > 1.0f*checkScaler) && (frontend->_gpsCheck & MASK_GPS_SPD_ERR);
159 :
160 : // Report check result as a text string and bitmask
161 : if (gpsSpdAccFail) {
162 340 : dal.snprintf(prearm_fail_string,
163 : sizeof(prearm_fail_string),
164 340 : "GPS speed error %.1f (needs < %.1f)", (double)gpsSpdAccuracy, (double)(1.0f*checkScaler));
165 340 : gpsCheckStatus.bad_sAcc = true;
166 : } else {
167 310306 : gpsCheckStatus.bad_sAcc = false;
168 : }
169 :
170 : // fail if satellite geometry is poor
171 310646 : bool hdopFail = (gps.get_hdop() > 250) && (frontend->_gpsCheck & MASK_GPS_HDOP);
172 :
173 : // Report check result as a text string and bitmask
174 : if (hdopFail) {
175 0 : dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string),
176 0 : "GPS HDOP %.1f (needs 2.5)", (double)(0.01f * gps.get_hdop()));
177 0 : gpsCheckStatus.bad_hdop = true;
178 : } else {
179 310646 : gpsCheckStatus.bad_hdop = false;
180 : }
181 :
182 : // fail if not enough sats
183 310646 : bool numSatsFail = (gps.num_sats() < 6) && (frontend->_gpsCheck & MASK_GPS_NSATS);
184 :
185 : // Report check result as a text string and bitmask
186 : if (numSatsFail) {
187 0 : dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string),
188 0 : "GPS numsats %u (needs 6)", gps.num_sats());
189 0 : gpsCheckStatus.bad_sats = true;
190 : } else {
191 310646 : gpsCheckStatus.bad_sats = false;
192 : }
193 :
194 : // fail if magnetometer innovations are outside limits indicating bad yaw
195 : // with bad yaw we are unable to use GPS
196 : bool yawFail;
197 310646 : if ((magTestRatio.x > 1.0f || magTestRatio.y > 1.0f || yawTestRatio > 1.0f) && (frontend->_gpsCheck & MASK_GPS_YAW_ERR)) {
198 : yawFail = true;
199 : } else {
200 : yawFail = false;
201 : }
202 :
203 : // Report check result as a text string and bitmask
204 : if (yawFail) {
205 2036 : dal.snprintf(prearm_fail_string,
206 : sizeof(prearm_fail_string),
207 : "Mag yaw error x=%.1f y=%.1f",
208 2036 : (double)magTestRatio.x,
209 2036 : (double)magTestRatio.y);
210 2036 : gpsCheckStatus.bad_yaw = true;
211 : } else {
212 308610 : gpsCheckStatus.bad_yaw = false;
213 : }
214 :
215 : // assume failed first time through and notify user checks have started
216 310646 : if (lastGpsVelFail_ms == 0) {
217 540 : dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "EKF starting GPS checks");
218 540 : lastGpsVelFail_ms = imuSampleTime_ms;
219 : }
220 :
221 : // record time of fail or pass
222 310646 : if (gpsSpdAccFail || numSatsFail || hdopFail || hAccFail || vAccFail || yawFail || gpsDriftFail || gpsVertVelFail || gpsHorizVelFail) {
223 2344 : lastGpsVelFail_ms = imuSampleTime_ms;
224 : } else {
225 308302 : lastGpsVelPass_ms = imuSampleTime_ms;
226 : }
227 :
228 : // continuous period of 10s without fail required to set healthy
229 : // continuous period of 5s without pass required to set unhealthy
230 310646 : if (!gpsGoodToAlign && imuSampleTime_ms - lastGpsVelFail_ms > 10000) {
231 400 : gpsGoodToAlign = true;
232 310246 : } else if (gpsGoodToAlign && imuSampleTime_ms - lastGpsVelPass_ms > 5000) {
233 16 : gpsGoodToAlign = false;
234 : }
235 : }
236 :
237 : // update inflight calculaton that determines if GPS data is good enough for reliable navigation
238 312142 : void NavEKF2_core::calcGpsGoodForFlight(void)
239 : {
240 : // use a simple criteria based on the GPS receivers claimed speed accuracy and the EKF innovation consistency checks
241 :
242 : // set up varaibles and constants used by filter that is applied to GPS speed accuracy
243 : const ftype alpha1 = 0.2f; // coefficient for first stage LPF applied to raw speed accuracy data
244 : const ftype tau = 10.0f; // time constant (sec) of peak hold decay
245 312142 : if (lastGpsCheckTime_ms == 0) {
246 540 : lastGpsCheckTime_ms = imuSampleTime_ms;
247 : }
248 312142 : ftype dtLPF = (imuSampleTime_ms - lastGpsCheckTime_ms) * 1e-3f;
249 312142 : lastGpsCheckTime_ms = imuSampleTime_ms;
250 312142 : ftype alpha2 = constrain_ftype(dtLPF/tau,0.0f,1.0f);
251 :
252 : // get the receivers reported speed accuracy
253 : float gpsSpdAccRaw;
254 312142 : if (!dal.gps().speed_accuracy(gpsSpdAccRaw)) {
255 0 : gpsSpdAccRaw = 0.0f;
256 : }
257 :
258 : // filter the raw speed accuracy using a LPF
259 312142 : sAccFilterState1 = constrain_ftype((alpha1 * gpsSpdAccRaw + (1.0f - alpha1) * sAccFilterState1),0.0f,10.0f);
260 :
261 : // apply a peak hold filter to the LPF output
262 312142 : sAccFilterState2 = MAX(sAccFilterState1,((1.0f - alpha2) * sAccFilterState2));
263 :
264 : // Apply a threshold test with hysteresis to the filtered GPS speed accuracy data
265 312142 : if (sAccFilterState2 > 1.5f ) {
266 138 : gpsSpdAccPass = false;
267 312004 : } else if(sAccFilterState2 < 1.0f) {
268 311912 : gpsSpdAccPass = true;
269 : }
270 :
271 : // Apply a threshold test with hysteresis to the normalised position and velocity innovations
272 : // Require a fail for one second and a pass for 10 seconds to transition
273 312142 : if (lastInnovFailTime_ms == 0) {
274 540 : lastInnovFailTime_ms = imuSampleTime_ms;
275 540 : lastInnovPassTime_ms = imuSampleTime_ms;
276 : }
277 312142 : if (velTestRatio < 1.0f && posTestRatio < 1.0f) {
278 309087 : lastInnovPassTime_ms = imuSampleTime_ms;
279 3055 : } else if (velTestRatio > 0.7f || posTestRatio > 0.7f) {
280 3055 : lastInnovFailTime_ms = imuSampleTime_ms;
281 : }
282 312142 : if ((imuSampleTime_ms - lastInnovPassTime_ms) > 1000) {
283 2774 : ekfInnovationsPass = false;
284 309368 : } else if ((imuSampleTime_ms - lastInnovFailTime_ms) > 10000) {
285 283276 : ekfInnovationsPass = true;
286 : }
287 :
288 : // both GPS speed accuracy and EKF innovations must pass
289 312142 : gpsAccuracyGood = gpsSpdAccPass && ekfInnovationsPass;
290 312142 : }
291 :
292 : // Detect if we are in flight or on ground
293 4213784 : void NavEKF2_core::detectFlight()
294 : {
295 : /*
296 : If we are a fly forward type vehicle (eg plane), then in-air status can be determined through a combination of speed and height criteria.
297 : Because of the differing certainty requirements of algorithms that need the in-flight / on-ground status we use two booleans where
298 : onGround indicates a high certainty we are not flying and inFlight indicates a high certainty we are flying. It is possible for
299 : both onGround and inFlight to be false if the status is uncertain, but they cannot both be true.
300 :
301 : If we are a plane as indicated by the assume_zero_sideslip() status, then different logic is used
302 :
303 : TODO - this logic should be moved out of the EKF and into the flight vehicle code.
304 : */
305 :
306 4213784 : if (assume_zero_sideslip()) {
307 : // To be confident we are in the air we use a criteria which combines arm status, ground speed, airspeed and height change
308 2981090 : ftype gndSpdSq = sq(gpsDataDelayed.vel.x) + sq(gpsDataDelayed.vel.y);
309 : bool highGndSpd = false;
310 : bool highAirSpd = false;
311 : bool largeHgtChange = false;
312 :
313 : // trigger at 8 m/s airspeed
314 2981090 : if (dal.airspeed_sensor_enabled()) {
315 2827888 : const auto *airspeed = dal.airspeed();
316 2827888 : if (airspeed->get_airspeed() * dal.get_EAS2TAS() > 10.0f) {
317 : highAirSpd = true;
318 : }
319 : }
320 :
321 : // trigger at 10 m/s GPS velocity, but not if GPS is reporting bad velocity errors
322 2981090 : if (gndSpdSq > 100.0f && gpsSpdAccuracy < 1.0f) {
323 : highGndSpd = true;
324 : }
325 :
326 : // trigger if more than 10m away from initial height
327 2981090 : if (fabsF(hgtMea) > 10.0f) {
328 : largeHgtChange = true;
329 : }
330 :
331 : // Determine to a high certainty we are flying
332 2981090 : if (motorsArmed && highGndSpd && (highAirSpd || largeHgtChange)) {
333 2023358 : onGround = false;
334 2023358 : inFlight = true;
335 : }
336 :
337 : // if is possible we are in flight, set the time this condition was last detected
338 2981090 : if (motorsArmed && (highGndSpd || highAirSpd || largeHgtChange)) {
339 2163288 : airborneDetectTime_ms = imuSampleTime_ms;
340 2163288 : onGround = false;
341 : }
342 :
343 : // Determine to a high certainty we are not flying
344 : // after 5 seconds of not detecting a possible flight condition or we are disarmed, we transition to on-ground mode
345 2981090 : if(!motorsArmed || ((imuSampleTime_ms - airborneDetectTime_ms) > 5000)) {
346 764138 : onGround = true;
347 764138 : inFlight = false;
348 : }
349 : } else {
350 : // Non fly forward vehicle, so can only use height and motor arm status
351 :
352 : // If the motors are armed then we could be flying and if they are not armed then we are definitely not flying
353 1232694 : if (motorsArmed) {
354 776306 : onGround = false;
355 : } else {
356 456388 : inFlight = false;
357 456388 : onGround = true;
358 : }
359 :
360 1232694 : if (!onGround) {
361 : // If height has increased since exiting on-ground, then we definitely are flying
362 776306 : if ((stateStruct.position.z - posDownAtTakeoff) < -1.5f) {
363 638299 : inFlight = true;
364 : }
365 :
366 : // If rangefinder has increased since exiting on-ground, then we definitely are flying
367 776306 : if ((rangeDataNew.rng - rngAtStartOfFlight) > 0.5f) {
368 86304 : inFlight = true;
369 : }
370 :
371 : // If more than 5 seconds since likely_flying was set
372 : // true, then set inFlight true
373 776306 : if (dal.get_time_flying_ms() > 5000) {
374 686758 : inFlight = true;
375 : }
376 : }
377 :
378 : }
379 :
380 : // handle reset of counters used to control how many times we will try to reset the yaw to the EKF-GSF value per flight
381 4213784 : if ((!prevOnGround && onGround) || !gpsSpdAccPass) {
382 : // disable filter bank
383 73592 : EKFGSF_run_filterbank = false;
384 4140192 : } else if (yawEstimator != nullptr && !EKFGSF_run_filterbank && inFlight && gpsSpdAccPass) {
385 : // flying so reset counters and enable filter bank when GPS is good
386 316 : EKFGSF_yaw_reset_ms = 0;
387 316 : EKFGSF_yaw_reset_request_ms = 0;
388 316 : EKFGSF_yaw_reset_count = 0;
389 316 : EKFGSF_run_filterbank = true;
390 316 : Vector3f gyroBias;
391 316 : getGyroBias(gyroBias);
392 316 : yawEstimator->setGyroBias(gyroBias);
393 : }
394 :
395 : // store current on-ground and in-air status for next time
396 4213784 : prevOnGround = onGround;
397 4213784 : prevInFlight = inFlight;
398 :
399 : // Store vehicle height and range prior to takeoff for use in post takeoff checks
400 4213784 : if (onGround) {
401 : // store vertical position at start of flight to use as a reference for ground relative checks
402 1220526 : posDownAtTakeoff = stateStruct.position.z;
403 : // store the range finder measurement which will be used as a reference to detect when we have taken off
404 1220526 : rngAtStartOfFlight = rangeDataNew.rng;
405 : // if the magnetic field states have been set, then continue to update the vertical position
406 : // quaternion and yaw innovation snapshots to use as a reference when we start to fly.
407 1220526 : if (magStateInitComplete) {
408 1216884 : posDownAtLastMagReset = stateStruct.position.z;
409 1216884 : quatAtLastMagReset = stateStruct.quat;
410 1216884 : yawInnovAtLastMagReset = innovYaw;
411 : }
412 : }
413 :
414 4213784 : }
415 :
416 :
417 : // Set to true if the terrain underneath is stable enough to be used as a height reference
418 : // in combination with a range finder. Set to false if the terrain underneath the vehicle
419 : // cannot be used as a height reference. Use to prevent range finder operation otherwise
420 : // enabled by the combination of EK2_RNG_AID_HGT and EK2_RNG_USE_SPD parameters.
421 56 : void NavEKF2_core::setTerrainHgtStable(bool val)
422 : {
423 56 : terrainHgtStable = val;
424 56 : }
425 :
426 : // Detect takeoff for optical flow navigation
427 0 : void NavEKF2_core::detectOptFlowTakeoff(void)
428 : {
429 0 : if (!onGround && !takeOffDetected && (imuSampleTime_ms - timeAtArming_ms) > 1000) {
430 : // we are no longer confidently on the ground so check the range finder and gyro for signs of takeoff
431 0 : const auto &ins = dal.ins();
432 0 : Vector3f angRateVec;
433 0 : Vector3f gyroBias;
434 0 : getGyroBias(gyroBias);
435 0 : angRateVec = ins.get_gyro(gyro_index_active) - gyroBias;
436 :
437 0 : takeOffDetected = (takeOffDetected || (angRateVec.length() > 0.1f) || (rangeDataNew.rng > (rngAtStartOfFlight + 0.1f)));
438 0 : } else if (onGround) {
439 : // we are confidently on the ground so set the takeoff detected status to false
440 0 : takeOffDetected = false;
441 : }
442 0 : }
443 :
|
