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