Line data Source code
1 : #include <AP_HAL/AP_HAL.h>
2 :
3 : #include "AP_NavEKF2.h"
4 : #include "AP_NavEKF2_core.h"
5 : #include <AP_DAL/AP_DAL.h>
6 :
7 : extern const AP_HAL::HAL& hal;
8 :
9 : /********************************************************
10 : * RESET FUNCTIONS *
11 : ********************************************************/
12 :
13 : // Reset XY velocity states to last GPS measurement if available or to zero if in constant position mode or if PV aiding is not absolute
14 : // Do not reset vertical velocity using GPS as there is baro alt available to constrain drift
15 860 : void NavEKF2_core::ResetVelocity(void)
16 : {
17 : // reset the corresponding covariances
18 860 : zeroRows(P,3,4);
19 860 : zeroCols(P,3,4);
20 :
21 860 : if (PV_AidingMode != AID_ABSOLUTE) {
22 562 : stateStruct.velocity.xy().zero();
23 : // set the variances using the measurement noise parameter
24 562 : P[4][4] = P[3][3] = sq(frontend->_gpsHorizVelNoise);
25 : } else {
26 : // reset horizontal velocity states to the GPS velocity if available
27 298 : if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
28 : // correct for antenna position
29 294 : gps_elements gps_corrected = gpsDataNew;
30 294 : CorrectGPSForAntennaOffset(gps_corrected);
31 294 : stateStruct.velocity.x = gps_corrected.vel.x;
32 294 : stateStruct.velocity.y = gps_corrected.vel.y;
33 : // set the variances using the reported GPS speed accuracy
34 294 : P[4][4] = P[3][3] = sq(MAX(frontend->_gpsHorizVelNoise,gpsSpdAccuracy));
35 4 : } else if (imuSampleTime_ms - extNavVelMeasTime_ms < 250) {
36 : // use external nav data as the 2nd preference
37 0 : stateStruct.velocity = extNavVelDelayed.vel;
38 0 : P[5][5] = P[4][4] = P[3][3] = sq(extNavVelDelayed.err);
39 : } else {
40 4 : stateStruct.velocity.x = 0.0f;
41 4 : stateStruct.velocity.y = 0.0f;
42 : // set the variances using the likely speed range
43 4 : P[4][4] = P[3][3] = sq(25.0f);
44 : }
45 : // clear the timeout flags and counters
46 298 : velTimeout = false;
47 298 : lastVelPassTime_ms = imuSampleTime_ms;
48 : }
49 13262 : for (uint8_t i=0; i<imu_buffer_length; i++) {
50 12402 : storedOutput[i].velocity.x = stateStruct.velocity.x;
51 12402 : storedOutput[i].velocity.y = stateStruct.velocity.y;
52 : }
53 860 : outputDataNew.velocity.x = stateStruct.velocity.x;
54 860 : outputDataNew.velocity.y = stateStruct.velocity.y;
55 860 : outputDataDelayed.velocity.x = stateStruct.velocity.x;
56 860 : outputDataDelayed.velocity.y = stateStruct.velocity.y;
57 :
58 :
59 :
60 860 : }
61 :
62 : // resets position states to last GPS measurement or to zero if in constant position mode
63 860 : void NavEKF2_core::ResetPosition(void)
64 : {
65 : // Store the position before the reset so that we can record the reset delta
66 860 : posResetNE.x = stateStruct.position.x;
67 860 : posResetNE.y = stateStruct.position.y;
68 :
69 : // reset the corresponding covariances
70 860 : zeroRows(P,6,7);
71 860 : zeroCols(P,6,7);
72 :
73 860 : if (PV_AidingMode != AID_ABSOLUTE) {
74 : // reset all position state history to the last known position
75 562 : stateStruct.position.x = lastKnownPositionNE.x;
76 562 : stateStruct.position.y = lastKnownPositionNE.y;
77 : // set the variances using the position measurement noise parameter
78 562 : P[6][6] = P[7][7] = sq(frontend->_gpsHorizPosNoise);
79 : } else {
80 : // Use GPS data as first preference if fresh data is available
81 298 : if (imuSampleTime_ms - lastTimeGpsReceived_ms < 250) {
82 : // correct for antenna position
83 294 : gps_elements gps_corrected = gpsDataNew;
84 294 : CorrectGPSForAntennaOffset(gps_corrected);
85 : // record the ID of the GPS for the data we are using for the reset
86 294 : last_gps_idx = gps_corrected.sensor_idx;
87 : // write to state vector and compensate for offset between last GPS measurement and the EKF time horizon
88 294 : stateStruct.position.x = gps_corrected.pos.x + 0.001f*gps_corrected.vel.x*(float(imuDataDelayed.time_ms) - float(gps_corrected.time_ms));
89 294 : stateStruct.position.y = gps_corrected.pos.y + 0.001f*gps_corrected.vel.y*(float(imuDataDelayed.time_ms) - float(gps_corrected.time_ms));
90 : // set the variances using the position measurement noise parameter
91 294 : P[6][6] = P[7][7] = sq(MAX(gpsPosAccuracy,frontend->_gpsHorizPosNoise));
92 : // clear the timeout flags and counters
93 294 : posTimeout = false;
94 294 : lastPosPassTime_ms = imuSampleTime_ms;
95 4 : } else if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250) {
96 : // use the range beacon data as a second preference
97 0 : stateStruct.position.x = receiverPos.x;
98 0 : stateStruct.position.y = receiverPos.y;
99 : // set the variances from the beacon alignment filter
100 0 : P[6][6] = receiverPosCov[0][0];
101 0 : P[7][7] = receiverPosCov[1][1];
102 : // clear the timeout flags and counters
103 0 : rngBcnTimeout = false;
104 0 : lastRngBcnPassTime_ms = imuSampleTime_ms;
105 4 : } else if (imuSampleTime_ms - extNavDataDelayed.time_ms < 250) {
106 : // use external nav data as the third preference
107 0 : ext_nav_elements extNavCorrected = extNavDataDelayed;
108 0 : CorrectExtNavForSensorOffset(extNavCorrected.pos);
109 0 : stateStruct.position.x = extNavCorrected.pos.x;
110 0 : stateStruct.position.y = extNavCorrected.pos.y;
111 : // set the variances from the external nav filter
112 0 : P[7][7] = P[6][6] = sq(extNavCorrected.posErr);
113 : }
114 : }
115 13262 : for (uint8_t i=0; i<imu_buffer_length; i++) {
116 12402 : storedOutput[i].position.x = stateStruct.position.x;
117 12402 : storedOutput[i].position.y = stateStruct.position.y;
118 : }
119 860 : outputDataNew.position.x = stateStruct.position.x;
120 860 : outputDataNew.position.y = stateStruct.position.y;
121 860 : outputDataDelayed.position.x = stateStruct.position.x;
122 860 : outputDataDelayed.position.y = stateStruct.position.y;
123 :
124 : // Calculate the position jump due to the reset
125 860 : posResetNE.x = stateStruct.position.x - posResetNE.x;
126 860 : posResetNE.y = stateStruct.position.y - posResetNE.y;
127 :
128 860 : posNEResetCount++;
129 :
130 860 : }
131 :
132 : // reset the stateStruct's NE position to the specified position
133 : // posResetNE is updated to hold the change in position
134 : // storedOutput, outputDataNew and outputDataDelayed are updated with the change in position
135 : // posNEResetCount is incremented to record the reset
136 0 : void NavEKF2_core::ResetPositionNE(ftype posN, ftype posE)
137 : {
138 : // Store the position before the reset so that we can record the reset delta
139 0 : const Vector3F posOrig = stateStruct.position;
140 :
141 : // Set the position states to the new position
142 0 : stateStruct.position.x = posN;
143 0 : stateStruct.position.y = posE;
144 :
145 : // Calculate the position offset due to the reset
146 0 : posResetNE.x = stateStruct.position.x - posOrig.x;
147 0 : posResetNE.y = stateStruct.position.y - posOrig.y;
148 :
149 : // Add the offset to the output observer states
150 0 : for (uint8_t i=0; i<imu_buffer_length; i++) {
151 0 : storedOutput[i].position.x += posResetNE.x;
152 0 : storedOutput[i].position.y += posResetNE.y;
153 : }
154 0 : outputDataNew.position.x += posResetNE.x;
155 0 : outputDataNew.position.y += posResetNE.y;
156 0 : outputDataDelayed.position.x += posResetNE.x;
157 0 : outputDataDelayed.position.y += posResetNE.y;
158 :
159 0 : posNEResetCount++;
160 0 : }
161 :
162 : // reset the vertical position state using the last height measurement
163 566 : void NavEKF2_core::ResetHeight(void)
164 : {
165 : // Store the position before the reset so that we can record the reset delta
166 566 : posResetD = stateStruct.position.z;
167 :
168 : // write to the state vector
169 566 : stateStruct.position.z = -hgtMea;
170 566 : outputDataNew.position.z = stateStruct.position.z;
171 566 : outputDataDelayed.position.z = stateStruct.position.z;
172 :
173 : // reset the terrain state height
174 566 : if (onGround) {
175 : // assume vehicle is sitting on the ground
176 560 : terrainState = stateStruct.position.z + rngOnGnd;
177 : } else {
178 : // can make no assumption other than vehicle is not below ground level
179 6 : terrainState = MAX(stateStruct.position.z + rngOnGnd , terrainState);
180 : }
181 8626 : for (uint8_t i=0; i<imu_buffer_length; i++) {
182 8060 : storedOutput[i].position.z = stateStruct.position.z;
183 : }
184 566 : vertCompFiltState.pos = stateStruct.position.z;
185 :
186 : // Calculate the position jump due to the reset
187 566 : posResetD = stateStruct.position.z - posResetD;
188 :
189 566 : posDResetCount++;
190 :
191 : // clear the timeout flags and counters
192 566 : hgtTimeout = false;
193 566 : lastHgtPassTime_ms = imuSampleTime_ms;
194 :
195 : // reset the corresponding covariances
196 566 : zeroRows(P,8,8);
197 566 : zeroCols(P,8,8);
198 :
199 : // set the variances to the measurement variance
200 566 : P[8][8] = posDownObsNoise;
201 :
202 : // Reset the vertical velocity state using GPS vertical velocity if we are airborne
203 : // Check that GPS vertical velocity data is available and can be used
204 566 : if (inFlight && !gpsNotAvailable && frontend->_fusionModeGPS == 0 &&
205 6 : dal.gps().have_vertical_velocity()) {
206 6 : stateStruct.velocity.z = gpsDataNew.vel.z;
207 560 : } else if (inFlight && useExtNavVel) {
208 0 : stateStruct.velocity.z = extNavVelNew.vel.z;
209 560 : } else if (onGround) {
210 560 : stateStruct.velocity.z = 0.0f;
211 : }
212 8626 : for (uint8_t i=0; i<imu_buffer_length; i++) {
213 8060 : storedOutput[i].velocity.z = stateStruct.velocity.z;
214 : }
215 566 : outputDataNew.velocity.z = stateStruct.velocity.z;
216 566 : outputDataDelayed.velocity.z = stateStruct.velocity.z;
217 566 : vertCompFiltState.vel = outputDataNew.velocity.z;
218 :
219 : // reset the corresponding covariances
220 566 : zeroRows(P,5,5);
221 566 : zeroCols(P,5,5);
222 :
223 : // set the variances to the measurement variance
224 566 : if (useExtNavVel) {
225 0 : P[5][5] = sq(extNavVelNew.err);
226 : } else {
227 566 : P[5][5] = sq(frontend->_gpsVertVelNoise);
228 : }
229 566 : }
230 :
231 : // reset the stateStruct's D position
232 : // posResetD is updated to hold the change in position
233 : // storedOutput, outputDataNew and outputDataDelayed are updated with the change in position
234 : // posDResetCount is incremented to record the reset
235 0 : void NavEKF2_core::ResetPositionD(ftype posD)
236 : {
237 : // Store the position before the reset so that we can record the reset delta
238 0 : const ftype posDOrig = stateStruct.position.z;
239 :
240 : // write to the state vector
241 0 : stateStruct.position.z = posD;
242 :
243 : // Calculate the position jump due to the reset
244 0 : posResetD = stateStruct.position.z - posDOrig;
245 :
246 : // Add the offset to the output observer states
247 0 : outputDataNew.position.z += posResetD;
248 0 : vertCompFiltState.pos = outputDataNew.position.z;
249 0 : outputDataDelayed.position.z += posResetD;
250 0 : for (uint8_t i=0; i<imu_buffer_length; i++) {
251 0 : storedOutput[i].position.z += posResetD;
252 : }
253 :
254 0 : posDResetCount++;
255 0 : }
256 :
257 : // Zero the EKF height datum
258 : // Return true if the height datum reset has been performed
259 2992 : bool NavEKF2_core::resetHeightDatum(void)
260 : {
261 2992 : if (activeHgtSource == HGT_SOURCE_RNG || !onGround) {
262 : // only allow resets when on the ground.
263 : // If using using rangefinder for height then never perform a
264 : // reset of the height datum
265 : return false;
266 : }
267 : // record the old height estimate
268 2986 : ftype oldHgt = -stateStruct.position.z;
269 : // reset the barometer so that it reads zero at the current height
270 2986 : dal.baro().update_calibration();
271 : // reset the height state
272 2986 : stateStruct.position.z = 0.0f;
273 : // adjust the height of the EKF origin so that the origin plus baro height before and after the reset is the same
274 :
275 2986 : if (validOrigin) {
276 2126 : if (!gpsGoodToAlign) {
277 : // if we don't have GPS lock then we shouldn't be doing a
278 : // resetHeightDatum, but if we do then the best option is
279 : // to maintain the old error
280 36 : EKF_origin.alt += (int32_t)(100.0f * oldHgt);
281 : } else {
282 : // if we have a good GPS lock then reset to the GPS
283 : // altitude. This ensures the reported AMSL alt from
284 : // getLLH() is equal to GPS altitude, while also ensuring
285 : // that the relative alt is zero
286 2090 : EKF_origin.copy_alt_from(dal.gps().location());
287 : }
288 2126 : ekfGpsRefHgt = (double)0.01 * (double)EKF_origin.alt;
289 : }
290 :
291 : // set the terrain state to zero (on ground). The adjustment for
292 : // frame height will get added in the later constraints
293 2986 : terrainState = 0;
294 2986 : return true;
295 : }
296 :
297 : /*
298 : correct GPS data for position offset of antenna phase centre relative to the IMU
299 : */
300 255468 : void NavEKF2_core::CorrectGPSForAntennaOffset(gps_elements &gps_data) const
301 : {
302 255468 : const Vector3F posOffsetBody = dal.gps().get_antenna_offset(gpsDataDelayed.sensor_idx).toftype() - accelPosOffset;
303 255468 : if (posOffsetBody.is_zero()) {
304 254258 : return;
305 : }
306 :
307 : // Don't fuse velocity data if GPS doesn't support it
308 1210 : if (fuseVelData) {
309 : // TODO use a filtered angular rate with a group delay that matches the GPS delay
310 1206 : Vector3F angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
311 1206 : Vector3F velOffsetBody = angRate % posOffsetBody;
312 1206 : Vector3F velOffsetEarth = prevTnb.mul_transpose(velOffsetBody);
313 1206 : gps_data.vel.x -= velOffsetEarth.x;
314 1206 : gps_data.vel.y -= velOffsetEarth.y;
315 1206 : gps_data.vel.z -= velOffsetEarth.z;
316 : }
317 :
318 1210 : Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
319 1210 : gps_data.pos.x -= posOffsetEarth.x;
320 1210 : gps_data.pos.y -= posOffsetEarth.y;
321 1210 : gps_data.hgt += posOffsetEarth.z;
322 : }
323 :
324 : // correct external navigation earth-frame position using sensor body-frame offset
325 0 : void NavEKF2_core::CorrectExtNavForSensorOffset(Vector3F &ext_position) const
326 : {
327 : #if HAL_VISUALODOM_ENABLED
328 0 : const auto *visual_odom = dal.visualodom();
329 0 : if (visual_odom == nullptr) {
330 0 : return;
331 : }
332 0 : const Vector3F posOffsetBody = visual_odom->get_pos_offset().toftype() - accelPosOffset;
333 0 : if (posOffsetBody.is_zero()) {
334 : return;
335 : }
336 0 : Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
337 0 : ext_position.x -= posOffsetEarth.x;
338 0 : ext_position.y -= posOffsetEarth.y;
339 0 : ext_position.z -= posOffsetEarth.z;
340 : #endif
341 : }
342 :
343 : // correct external navigation earth-frame velocity using sensor body-frame offset
344 0 : void NavEKF2_core::CorrectExtNavVelForSensorOffset(Vector3F &ext_velocity) const
345 : {
346 : #if HAL_VISUALODOM_ENABLED
347 0 : const auto *visual_odom = dal.visualodom();
348 0 : if (visual_odom == nullptr) {
349 0 : return;
350 : }
351 0 : const Vector3F posOffsetBody = visual_odom->get_pos_offset().toftype() - accelPosOffset;
352 0 : if (posOffsetBody.is_zero()) {
353 : return;
354 : }
355 : // TODO use a filtered angular rate with a group delay that matches the sensor delay
356 0 : const Vector3F angRate = imuDataDelayed.delAng * (1.0f/imuDataDelayed.delAngDT);
357 0 : ext_velocity += get_vel_correction_for_sensor_offset(posOffsetBody, prevTnb, angRate);
358 : #endif
359 : }
360 :
361 : /********************************************************
362 : * FUSE MEASURED_DATA *
363 : ********************************************************/
364 : // select fusion of velocity, position and height measurements
365 3248540 : void NavEKF2_core::SelectVelPosFusion()
366 : {
367 : // Check if the magnetometer has been fused on that time step and the filter is running at faster than 200 Hz
368 : // If so, don't fuse measurements on this time step to reduce frame over-runs
369 : // Only allow one time slip to prevent high rate magnetometer data preventing fusion of other measurements
370 3248540 : if (magFusePerformed && dtIMUavg < 0.005f && !posVelFusionDelayed) {
371 18168 : posVelFusionDelayed = true;
372 18168 : return;
373 : } else {
374 3230372 : posVelFusionDelayed = false;
375 : }
376 :
377 : // Check for data at the fusion time horizon
378 3230372 : extNavDataToFuse = storedExtNav.recall(extNavDataDelayed, imuDataDelayed.time_ms);
379 3230372 : extNavVelToFuse = storedExtNavVel.recall(extNavVelDelayed, imuDataDelayed.time_ms);
380 3230372 : if (extNavVelToFuse) {
381 0 : CorrectExtNavVelForSensorOffset(extNavVelDelayed.vel);
382 : }
383 :
384 : // read GPS data from the sensor and check for new data in the buffer
385 3230372 : readGpsData();
386 3230372 : gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
387 :
388 : // Determine if we need to fuse position and velocity data on this time step
389 3230372 : if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
390 : // set fusion request flags
391 254880 : if (frontend->_fusionModeGPS <= 1) {
392 254880 : fuseVelData = true;
393 : } else {
394 0 : fuseVelData = false;
395 : }
396 254880 : fusePosData = true;
397 254880 : extNavUsedForPos = false;
398 :
399 : // correct for antenna position
400 254880 : CorrectGPSForAntennaOffset(gpsDataDelayed);
401 :
402 : // copy corrected GPS data to observation vector
403 254880 : if (fuseVelData) {
404 254880 : velPosObs[0] = gpsDataDelayed.vel.x;
405 254880 : velPosObs[1] = gpsDataDelayed.vel.y;
406 254880 : velPosObs[2] = gpsDataDelayed.vel.z;
407 : }
408 254880 : velPosObs[3] = gpsDataDelayed.pos.x;
409 254880 : velPosObs[4] = gpsDataDelayed.pos.y;
410 :
411 2975492 : } else if (extNavDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
412 : // This is a special case that uses and external nav system for position
413 0 : extNavUsedForPos = true;
414 0 : activeHgtSource = HGT_SOURCE_EXTNAV;
415 0 : fuseVelData = false;
416 0 : fuseHgtData = true;
417 0 : fusePosData = true;
418 :
419 : // correct for external navigation sensor position
420 0 : CorrectExtNavForSensorOffset(extNavDataDelayed.pos);
421 :
422 0 : velPosObs[3] = extNavDataDelayed.pos.x;
423 0 : velPosObs[4] = extNavDataDelayed.pos.y;
424 0 : velPosObs[5] = extNavDataDelayed.pos.z;
425 :
426 : // if compass is disabled, also use it for yaw
427 0 : if (!use_compass()) {
428 0 : extNavUsedForYaw = true;
429 0 : if (!yawAlignComplete) {
430 0 : extNavYawResetRequest = true;
431 0 : magYawResetRequest = false;
432 0 : gpsYawResetRequest = false;
433 0 : controlMagYawReset();
434 0 : finalInflightYawInit = true;
435 : } else {
436 0 : fuseEulerYaw();
437 : }
438 : } else {
439 0 : extNavUsedForYaw = false;
440 : }
441 :
442 : } else {
443 2975492 : fuseVelData = false;
444 2975492 : fusePosData = false;
445 : }
446 :
447 3230372 : if (extNavVelToFuse && (frontend->_fusionModeGPS == 3)) {
448 0 : fuseVelData = true;
449 0 : velPosObs[0] = extNavVelDelayed.vel.x;
450 0 : velPosObs[1] = extNavVelDelayed.vel.y;
451 0 : velPosObs[2] = extNavVelDelayed.vel.z;
452 : }
453 :
454 : // we have GPS data to fuse and a request to align the yaw using the GPS course
455 3230372 : if (gpsYawResetRequest) {
456 2080921 : realignYawGPS();
457 : }
458 :
459 : // Select height data to be fused from the available baro, range finder and GPS sources
460 :
461 3230372 : selectHeightForFusion();
462 :
463 : // if we are using GPS, check for a change in receiver and reset position and height
464 3230372 : if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE && gpsDataDelayed.sensor_idx != last_gps_idx) {
465 : // record the ID of the GPS that we are using for the reset
466 0 : last_gps_idx = gpsDataDelayed.sensor_idx;
467 :
468 : // Store the position before the reset so that we can record the reset delta
469 0 : posResetNE.x = stateStruct.position.x;
470 0 : posResetNE.y = stateStruct.position.y;
471 :
472 : // Set the position states to the position from the new GPS
473 0 : stateStruct.position.x = gpsDataDelayed.pos.x;
474 0 : stateStruct.position.y = gpsDataDelayed.pos.y;
475 :
476 : // Calculate the position offset due to the reset
477 0 : posResetNE.x = stateStruct.position.x - posResetNE.x;
478 0 : posResetNE.y = stateStruct.position.y - posResetNE.y;
479 :
480 : // Add the offset to the output observer states
481 0 : for (uint8_t i=0; i<imu_buffer_length; i++) {
482 0 : storedOutput[i].position.x += posResetNE.x;
483 0 : storedOutput[i].position.y += posResetNE.y;
484 : }
485 0 : outputDataNew.position.x += posResetNE.x;
486 0 : outputDataNew.position.y += posResetNE.y;
487 0 : outputDataDelayed.position.x += posResetNE.x;
488 0 : outputDataDelayed.position.y += posResetNE.y;
489 :
490 0 : posNEResetCount++;
491 :
492 : // If we are also using GPS as the height reference, reset the height
493 0 : if (activeHgtSource == HGT_SOURCE_GPS) {
494 : // Store the position before the reset so that we can record the reset delta
495 0 : posResetD = stateStruct.position.z;
496 :
497 : // write to the state vector
498 0 : stateStruct.position.z = -hgtMea;
499 :
500 : // Calculate the position jump due to the reset
501 0 : posResetD = stateStruct.position.z - posResetD;
502 :
503 : // Add the offset to the output observer states
504 0 : outputDataNew.position.z += posResetD;
505 0 : vertCompFiltState.pos = outputDataNew.position.z;
506 0 : outputDataDelayed.position.z += posResetD;
507 0 : for (uint8_t i=0; i<imu_buffer_length; i++) {
508 0 : storedOutput[i].position.z += posResetD;
509 : }
510 :
511 0 : posDResetCount++;
512 : }
513 : }
514 :
515 : // check for external nav position reset
516 3230372 : if (extNavDataToFuse && (PV_AidingMode == AID_ABSOLUTE) && (frontend->_fusionModeGPS == 3) && extNavDataDelayed.posReset) {
517 0 : ResetPositionNE(extNavDataDelayed.pos.x, extNavDataDelayed.pos.y);
518 0 : if (activeHgtSource == HGT_SOURCE_EXTNAV) {
519 0 : ResetPositionD(-hgtMea);
520 : }
521 : }
522 :
523 : // If we are operating without any aiding, fuse in the last known position
524 : // to constrain tilt drift. This assumes a non-manoeuvring vehicle
525 : // Do this to coincide with the height fusion
526 3230372 : if (fuseHgtData && PV_AidingMode == AID_NONE) {
527 106637 : velPosObs[3] = lastKnownPositionNE.x;
528 106637 : velPosObs[4] = lastKnownPositionNE.y;
529 106637 : fusePosData = true;
530 106637 : fuseVelData = false;
531 : }
532 :
533 : // perform fusion
534 3230372 : if (fuseVelData || fusePosData || fuseHgtData) {
535 861141 : FuseVelPosNED();
536 : // clear the flags to prevent repeated fusion of the same data
537 861141 : fuseVelData = false;
538 861141 : fuseHgtData = false;
539 861141 : fusePosData = false;
540 : }
541 : }
542 :
543 : // fuse selected position, velocity and height measurements
544 861141 : void NavEKF2_core::FuseVelPosNED()
545 : {
546 : // health is set bad until test passed
547 : bool velHealth = false; // boolean true if velocity measurements have passed innovation consistency check
548 : bool posHealth = false; // boolean true if position measurements have passed innovation consistency check
549 : bool hgtHealth = false; // boolean true if height measurements have passed innovation consistency check
550 :
551 : // declare variables used to check measurement errors
552 861141 : Vector3F velInnov;
553 :
554 : // declare variables used to control access to arrays
555 861141 : bool fuseData[6] = {false,false,false,false,false,false};
556 : uint8_t stateIndex;
557 : uint8_t obsIndex;
558 :
559 : // declare variables used by state and covariance update calculations
560 : Vector6 R_OBS; // Measurement variances used for fusion
561 : Vector6 R_OBS_DATA_CHECKS; // Measurement variances used for data checks only
562 : ftype SK;
563 :
564 : // perform sequential fusion of GPS measurements. This assumes that the
565 : // errors in the different velocity and position components are
566 : // uncorrelated which is not true, however in the absence of covariance
567 : // data from the GPS receiver it is the only assumption we can make
568 : // so we might as well take advantage of the computational efficiencies
569 : // associated with sequential fusion
570 861141 : if (fuseVelData || fusePosData || fuseHgtData) {
571 :
572 : // calculate additional error in GPS position caused by manoeuvring
573 861141 : ftype posErr = frontend->gpsPosVarAccScale * accNavMag;
574 :
575 : // estimate the GPS Velocity, GPS horiz position and height measurement variances.
576 : // Use different errors if operating without external aiding using an assumed position or velocity of zero
577 861141 : if (PV_AidingMode == AID_NONE) {
578 106637 : if (tiltAlignComplete && motorsArmed) {
579 : // This is a compromise between corrections for gyro errors and reducing effect of manoeuvre accelerations on tilt estimate
580 33425 : R_OBS[0] = sq(constrain_ftype(frontend->_noaidHorizNoise, 0.5f, 50.0f));
581 : } else {
582 : // Use a smaller value to give faster initial alignment
583 73212 : R_OBS[0] = sq(0.5f);
584 : }
585 106637 : R_OBS[1] = R_OBS[0];
586 106637 : R_OBS[2] = R_OBS[0];
587 106637 : R_OBS[3] = R_OBS[0];
588 106637 : R_OBS[4] = R_OBS[0];
589 426548 : for (uint8_t i=0; i<=2; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
590 : } else {
591 754504 : if (gpsSpdAccuracy > 0.0f) {
592 : // use GPS receivers reported speed accuracy if available and floor at value set by GPS velocity noise parameter
593 754504 : R_OBS[0] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsHorizVelNoise, 50.0f));
594 754504 : R_OBS[2] = sq(constrain_ftype(gpsSpdAccuracy, frontend->_gpsVertVelNoise, 50.0f));
595 0 : } else if (extNavVelToFuse) {
596 0 : R_OBS[2] = R_OBS[0] = sq(constrain_ftype(extNavVelDelayed.err, 0.05f, 5.0f));
597 : } else {
598 : // calculate additional error in GPS velocity caused by manoeuvring
599 0 : R_OBS[0] = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
600 0 : R_OBS[2] = sq(constrain_ftype(frontend->_gpsVertVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsDVelVarAccScale * accNavMag);
601 : }
602 754504 : R_OBS[1] = R_OBS[0];
603 : // Use GPS reported position accuracy if available and floor at value set by GPS position noise parameter
604 754504 : if (gpsPosAccuracy > 0.0f) {
605 754504 : R_OBS[3] = sq(constrain_ftype(gpsPosAccuracy, frontend->_gpsHorizPosNoise, 100.0f));
606 0 : } else if (extNavUsedForPos) {
607 0 : R_OBS[3] = sq(constrain_ftype(extNavDataDelayed.posErr, 0.01f, 10.0f));
608 : } else {
609 0 : R_OBS[3] = sq(constrain_ftype(frontend->_gpsHorizPosNoise, 0.1f, 10.0f)) + sq(posErr);
610 : }
611 754504 : R_OBS[4] = R_OBS[3];
612 : // For data integrity checks we use the same measurement variances as used to calculate the Kalman gains for all measurements except GPS horizontal velocity
613 : // For horizontal GPS velocity we don't want the acceptance radius to increase with reported GPS accuracy so we use a value based on best GPS perfomrance
614 : // plus a margin for manoeuvres. It is better to reject GPS horizontal velocity errors early
615 : ftype obs_data_chk;
616 754504 : if (extNavVelToFuse) {
617 0 : obs_data_chk = sq(constrain_ftype(extNavVelDelayed.err, 0.05f, 5.0f)) + sq(frontend->extNavVelVarAccScale * accNavMag);
618 : } else {
619 754504 : obs_data_chk = sq(constrain_ftype(frontend->_gpsHorizVelNoise, 0.05f, 5.0f)) + sq(frontend->gpsNEVelVarAccScale * accNavMag);
620 : }
621 754504 : R_OBS_DATA_CHECKS[0] = R_OBS_DATA_CHECKS[1] = R_OBS_DATA_CHECKS[2] = obs_data_chk;
622 : }
623 861141 : R_OBS[5] = posDownObsNoise;
624 3444564 : for (uint8_t i=3; i<=5; i++) R_OBS_DATA_CHECKS[i] = R_OBS[i];
625 :
626 : // if vertical GPS velocity data and an independent height source is being used, check to see if the GPS vertical velocity and altimeter
627 : // innovations have the same sign and are outside limits. If so, then it is likely aliasing is affecting
628 : // the accelerometers and we should disable the GPS and barometer innovation consistency checks.
629 861141 : if (useGpsVertVel && fuseVelData && (frontend->_altSource != 2)) {
630 : // calculate innovations for height and vertical GPS vel measurements
631 254880 : ftype hgtErr = stateStruct.position.z - velPosObs[5];
632 254880 : ftype velDErr = stateStruct.velocity.z - velPosObs[2];
633 : // check if they are the same sign and both more than 3-sigma out of bounds
634 254880 : if ((hgtErr*velDErr > 0.0f) && (sq(hgtErr) > 9.0f * (P[8][8] + R_OBS_DATA_CHECKS[5])) && (sq(velDErr) > 9.0f * (P[5][5] + R_OBS_DATA_CHECKS[2]))) {
635 741 : badIMUdata = true;
636 : } else {
637 254139 : badIMUdata = false;
638 : }
639 : }
640 :
641 : // calculate innovations and check GPS data validity using an innovation consistency check
642 : // test position measurements
643 861141 : if (fusePosData) {
644 : // test horizontal position measurements
645 361517 : innovVelPos[3] = stateStruct.position.x - velPosObs[3];
646 361517 : innovVelPos[4] = stateStruct.position.y - velPosObs[4];
647 361517 : varInnovVelPos[3] = P[6][6] + R_OBS_DATA_CHECKS[3];
648 361517 : varInnovVelPos[4] = P[7][7] + R_OBS_DATA_CHECKS[4];
649 : // apply an innovation consistency threshold test, but don't fail if bad IMU data
650 361517 : ftype maxPosInnov2 = sq(MAX(0.01f * (ftype)frontend->_gpsPosInnovGate, 1.0f))*(varInnovVelPos[3] + varInnovVelPos[4]);
651 361517 : posTestRatio = (sq(innovVelPos[3]) + sq(innovVelPos[4])) / maxPosInnov2;
652 361517 : posHealth = ((posTestRatio < 1.0f) || badIMUdata);
653 : // use position data if healthy or timed out
654 361517 : if (PV_AidingMode == AID_NONE) {
655 : posHealth = true;
656 106637 : lastPosPassTime_ms = imuSampleTime_ms;
657 254880 : } else if (posHealth || posTimeout) {
658 : posHealth = true;
659 253696 : lastPosPassTime_ms = imuSampleTime_ms;
660 : // if timed out or outside the specified uncertainty radius, reset to the GPS
661 253696 : if (posTimeout || ((P[6][6] + P[7][7]) > sq(float(frontend->_gpsGlitchRadiusMax)))) {
662 : // reset the position to the current GPS position
663 36 : ResetPosition();
664 : // reset the velocity to the GPS velocity
665 36 : ResetVelocity();
666 : // don't fuse GPS data on this time step
667 36 : fusePosData = false;
668 36 : fuseVelData = false;
669 : // Reset the position variances and corresponding covariances to a value that will pass the checks
670 36 : zeroRows(P,6,7);
671 36 : zeroCols(P,6,7);
672 36 : P[6][6] = sq(float(0.5f*frontend->_gpsGlitchRadiusMax));
673 36 : P[7][7] = P[6][6];
674 : // Reset the normalised innovation to avoid failing the bad fusion tests
675 36 : posTestRatio = 0.0f;
676 36 : velTestRatio = 0.0f;
677 : }
678 : }
679 : }
680 :
681 : // test velocity measurements
682 861141 : if (fuseVelData) {
683 : // test velocity measurements
684 : uint8_t imax = 2;
685 : // Don't fuse vertical velocity observations if inhibited by the user or if we are using synthetic data
686 254844 : if (!useExtNavVel && (frontend->_fusionModeGPS > 0 || PV_AidingMode != AID_ABSOLUTE ||
687 254844 : !dal.gps().have_vertical_velocity())) {
688 : imax = 1;
689 : }
690 : ftype innovVelSumSq = 0; // sum of squares of velocity innovations
691 : ftype varVelSum = 0; // sum of velocity innovation variances
692 1019376 : for (uint8_t i = 0; i<=imax; i++) {
693 : // velocity states start at index 3
694 764532 : stateIndex = i + 3;
695 : // calculate innovations using blended and single IMU predicted states
696 764532 : velInnov[i] = stateStruct.velocity[i] - velPosObs[i]; // blended
697 : // calculate innovation variance
698 764532 : varInnovVelPos[i] = P[stateIndex][stateIndex] + R_OBS_DATA_CHECKS[i];
699 : // sum the innovation and innovation variances
700 764532 : innovVelSumSq += sq(velInnov[i]);
701 764532 : varVelSum += varInnovVelPos[i];
702 : }
703 : // apply an innovation consistency threshold test, but don't fail if bad IMU data
704 : // calculate the test ratio
705 254844 : velTestRatio = innovVelSumSq / (varVelSum * sq(MAX(0.01f * (ftype)frontend->_gpsVelInnovGate, 1.0f)));
706 : // fail if the ratio is greater than 1
707 254844 : velHealth = ((velTestRatio < 1.0f) || badIMUdata);
708 : // use velocity data if healthy, timed out, or in constant position mode
709 1493 : if (velHealth || velTimeout) {
710 : velHealth = true;
711 : // restart the timeout count
712 253351 : lastVelPassTime_ms = imuSampleTime_ms;
713 : // If we are doing full aiding and velocity fusion times out, reset to the GPS velocity
714 253351 : if (PV_AidingMode == AID_ABSOLUTE && velTimeout) {
715 : // reset the velocity to the GPS velocity
716 0 : ResetVelocity();
717 : // don't fuse GPS velocity data on this time step
718 0 : fuseVelData = false;
719 : // Reset the normalised innovation to avoid failing the bad fusion tests
720 0 : velTestRatio = 0.0f;
721 : }
722 : }
723 : }
724 :
725 : // test height measurements
726 861141 : if (fuseHgtData) {
727 : // calculate height innovations
728 614058 : innovVelPos[5] = stateStruct.position.z - velPosObs[5];
729 614058 : varInnovVelPos[5] = P[8][8] + R_OBS_DATA_CHECKS[5];
730 : // calculate the innovation consistency test ratio
731 614058 : hgtTestRatio = sq(innovVelPos[5]) / (sq(MAX(0.01f * (ftype)frontend->_hgtInnovGate, 1.0f)) * varInnovVelPos[5]);
732 :
733 : // when on ground we accept a larger test ratio to allow
734 : // the filter to handle large switch on IMU bias errors
735 : // without rejecting the height sensor
736 614058 : const ftype maxTestRatio = (PV_AidingMode == AID_NONE && onGround)? 3.0 : 1.0;
737 :
738 : // fail if the ratio is > maxTestRatio, but don't fail if bad IMU data
739 614058 : hgtHealth = (hgtTestRatio < maxTestRatio) || badIMUdata;
740 :
741 : // Fuse height data if healthy or timed out or in constant position mode
742 574 : if (hgtHealth || hgtTimeout) {
743 : // Calculate a filtered value to be used by pre-flight health checks
744 : // We need to filter because wind gusts can generate significant baro noise and we want to be able to detect bias errors in the inertial solution
745 613490 : if (onGround) {
746 163622 : ftype dtBaro = (imuSampleTime_ms - lastHgtPassTime_ms)*1.0e-3f;
747 : const ftype hgtInnovFiltTC = 2.0f;
748 163622 : ftype alpha = constrain_ftype(dtBaro/(dtBaro+hgtInnovFiltTC),0.0f,1.0f);
749 163622 : hgtInnovFiltState += (innovVelPos[5]-hgtInnovFiltState)*alpha;
750 : } else {
751 449868 : hgtInnovFiltState = 0.0f;
752 : }
753 :
754 : // if timed out, reset the height
755 613490 : if (hgtTimeout) {
756 8 : ResetHeight();
757 : }
758 :
759 : // If we have got this far then declare the height data as healthy and reset the timeout counter
760 : hgtHealth = true;
761 613490 : lastHgtPassTime_ms = imuSampleTime_ms;
762 : }
763 : }
764 :
765 : // set range for sequential fusion of velocity and position measurements depending on which data is available and its health
766 861141 : if (fuseVelData && velHealth) {
767 253351 : fuseData[0] = true;
768 253351 : fuseData[1] = true;
769 253351 : if (useGpsVertVel || useExtNavVel) {
770 253351 : fuseData[2] = true;
771 : }
772 253351 : tiltErrVec.zero();
773 : }
774 861141 : if (fusePosData && posHealth) {
775 360297 : fuseData[3] = true;
776 360297 : fuseData[4] = true;
777 360297 : tiltErrVec.zero();
778 : }
779 861141 : if (fuseHgtData && hgtHealth) {
780 613490 : fuseData[5] = true;
781 : }
782 :
783 : // fuse measurements sequentially
784 6027987 : for (obsIndex=0; obsIndex<=5; obsIndex++) {
785 5166846 : if (fuseData[obsIndex]) {
786 2094137 : stateIndex = 3 + obsIndex;
787 : // calculate the measurement innovation, using states from a different time coordinate if fusing height data
788 : // adjust scaling on GPS measurement noise variances if not enough satellites
789 2094137 : if (obsIndex <= 2)
790 : {
791 760053 : innovVelPos[obsIndex] = stateStruct.velocity[obsIndex] - velPosObs[obsIndex];
792 760053 : R_OBS[obsIndex] *= sq(gpsNoiseScaler);
793 : }
794 1334084 : else if (obsIndex == 3 || obsIndex == 4) {
795 720594 : innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
796 720594 : R_OBS[obsIndex] *= sq(gpsNoiseScaler);
797 : } else if (obsIndex == 5) {
798 613490 : innovVelPos[obsIndex] = stateStruct.position[obsIndex-3] - velPosObs[obsIndex];
799 : const ftype gndMaxBaroErr = 4.0f;
800 : const ftype gndBaroInnovFloor = -0.5f;
801 :
802 613490 : if(dal.get_touchdown_expected() && activeHgtSource == HGT_SOURCE_BARO) {
803 : // when a touchdown is expected, floor the barometer innovation at gndBaroInnovFloor
804 : // constrain the correction between 0 and gndBaroInnovFloor+gndMaxBaroErr
805 : // this function looks like this:
806 : // |/
807 : //---------|---------
808 : // ____/|
809 : // / |
810 : // / |
811 2034 : innovVelPos[5] += constrain_ftype(-innovVelPos[5]+gndBaroInnovFloor, 0.0f, gndBaroInnovFloor+gndMaxBaroErr);
812 : }
813 : }
814 :
815 : // calculate the Kalman gain and calculate innovation variances
816 2094137 : varInnovVelPos[obsIndex] = P[stateIndex][stateIndex] + R_OBS[obsIndex];
817 2094137 : SK = 1.0f/varInnovVelPos[obsIndex];
818 35600329 : for (uint8_t i= 0; i<=15; i++) {
819 33506192 : Kfusion[i] = P[i][stateIndex]*SK;
820 : }
821 :
822 : // inhibit magnetic field state estimation by setting Kalman gains to zero
823 2094137 : if (!inhibitMagStates) {
824 10699633 : for (uint8_t i = 16; i<=21; i++) {
825 9171114 : Kfusion[i] = P[i][stateIndex]*SK;
826 : }
827 : } else {
828 3959326 : for (uint8_t i = 16; i<=21; i++) {
829 3393708 : Kfusion[i] = 0.0f;
830 : }
831 : }
832 :
833 : // inhibit wind state estimation by setting Kalman gains to zero
834 2094137 : if (!inhibitWindStates) {
835 1419073 : Kfusion[22] = P[22][stateIndex]*SK;
836 1419073 : Kfusion[23] = P[23][stateIndex]*SK;
837 : } else {
838 675064 : Kfusion[22] = 0.0f;
839 675064 : Kfusion[23] = 0.0f;
840 : }
841 :
842 : // update the covariance - take advantage of direct observation of a single state at index = stateIndex to reduce computations
843 : // this is a numerically optimised implementation of standard equation P = (I - K*H)*P;
844 47692575 : for (uint8_t i= 0; i<=stateIndexLim; i++) {
845 1063908026 : for (uint8_t j= 0; j<=stateIndexLim; j++)
846 : {
847 1018309588 : KHP[i][j] = Kfusion[i] * P[stateIndex][j];
848 : }
849 : }
850 : // Check that we are not going to drive any variances negative and skip the update if so
851 : bool healthyFusion = true;
852 47692575 : for (uint8_t i= 0; i<=stateIndexLim; i++) {
853 45598438 : if (KHP[i][i] > P[i][i]) {
854 : healthyFusion = false;
855 : }
856 : }
857 2094137 : if (healthyFusion) {
858 : // update the covariance matrix
859 47692507 : for (uint8_t i= 0; i<=stateIndexLim; i++) {
860 1063906938 : for (uint8_t j= 0; j<=stateIndexLim; j++) {
861 1018308564 : P[i][j] = P[i][j] - KHP[i][j];
862 : }
863 : }
864 :
865 : // force the covariance matrix to be symmetrical and limit the variances to prevent ill-conditioning.
866 2094133 : ForceSymmetry();
867 2094133 : ConstrainVariances();
868 :
869 : // update the states
870 : // zero the attitude error state - by definition it is assumed to be zero before each observation fusion
871 2094133 : stateStruct.angErr.zero();
872 :
873 : // calculate state corrections and re-normalise the quaternions for states predicted using the blended IMU data
874 47692507 : for (uint8_t i = 0; i<=stateIndexLim; i++) {
875 45598374 : statesArray[i] = statesArray[i] - Kfusion[i] * innovVelPos[obsIndex];
876 : }
877 :
878 : // the first 3 states represent the angular misalignment vector.
879 : // This is used to correct the estimated quaternion
880 2094133 : stateStruct.quat.rotate(stateStruct.angErr);
881 :
882 : // sum the attitude error from velocity and position fusion only
883 : // used as a metric for convergence monitoring
884 2094133 : if (obsIndex != 5) {
885 1480643 : tiltErrVec += stateStruct.angErr;
886 : }
887 : // record good fusion status
888 2094133 : if (obsIndex == 0) {
889 253351 : faultStatus.bad_nvel = false;
890 1840782 : } else if (obsIndex == 1) {
891 253347 : faultStatus.bad_evel = false;
892 1587435 : } else if (obsIndex == 2) {
893 253351 : faultStatus.bad_dvel = false;
894 1334084 : } else if (obsIndex == 3) {
895 360297 : faultStatus.bad_npos = false;
896 973787 : } else if (obsIndex == 4) {
897 360297 : faultStatus.bad_epos = false;
898 : } else if (obsIndex == 5) {
899 613490 : faultStatus.bad_dpos = false;
900 : }
901 : } else {
902 : // record bad fusion status
903 : if (obsIndex == 0) {
904 0 : faultStatus.bad_nvel = true;
905 : } else if (obsIndex == 1) {
906 4 : faultStatus.bad_evel = true;
907 : } else if (obsIndex == 2) {
908 0 : faultStatus.bad_dvel = true;
909 : } else if (obsIndex == 3) {
910 0 : faultStatus.bad_npos = true;
911 : } else if (obsIndex == 4) {
912 0 : faultStatus.bad_epos = true;
913 : } else if (obsIndex == 5) {
914 0 : faultStatus.bad_dpos = true;
915 : }
916 : }
917 : }
918 : }
919 : }
920 861141 : }
921 :
922 : /********************************************************
923 : * MISC FUNCTIONS *
924 : ********************************************************/
925 :
926 : // select the height measurement to be fused from the available baro, range finder and GPS sources
927 3230372 : void NavEKF2_core::selectHeightForFusion()
928 : {
929 : #if AP_RANGEFINDER_ENABLED
930 : // Read range finder data and check for new data in the buffer
931 : // This data is used by both height and optical flow fusion processing
932 3230372 : readRangeFinder();
933 3230372 : rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms);
934 :
935 : // correct range data for the body frame position offset relative to the IMU
936 : // the corrected reading is the reading that would have been taken if the sensor was
937 : // co-located with the IMU
938 3230372 : const auto *_rng = dal.rangefinder();
939 3230372 : if (_rng && rangeDataToFuse) {
940 3096 : const auto *sensor = _rng->get_backend(rangeDataDelayed.sensor_idx);
941 3096 : if (sensor != nullptr) {
942 3096 : Vector3F posOffsetBody = sensor->get_pos_offset().toftype() - accelPosOffset;
943 3096 : if (!posOffsetBody.is_zero()) {
944 1112 : Vector3F posOffsetEarth = prevTnb.mul_transpose(posOffsetBody);
945 1112 : rangeDataDelayed.rng += posOffsetEarth.z / prevTnb.c.z;
946 : }
947 : }
948 : }
949 : #endif
950 :
951 : // read baro height data from the sensor and check for new data in the buffer
952 3230372 : readBaroData();
953 3230372 : baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
954 :
955 3230372 : bool rangeFinderDataIsFresh = (imuSampleTime_ms - rngValidMeaTime_ms < 500);
956 : // select height source
957 3230372 : if (extNavUsedForPos) {
958 : // always use external navigation as the height source if using for position.
959 0 : activeHgtSource = HGT_SOURCE_EXTNAV;
960 : #if AP_RANGEFINDER_ENABLED
961 3230372 : } else if ((frontend->_altSource == 1) && _rng && rangeFinderDataIsFresh) {
962 : // user has specified the range finder as a primary height source
963 0 : activeHgtSource = HGT_SOURCE_RNG;
964 3230372 : } else if ((frontend->_useRngSwHgt > 0) && ((frontend->_altSource == 0) || (frontend->_altSource == 2)) && _rng && rangeFinderDataIsFresh) {
965 : // determine if we are above or below the height switch region
966 0 : const ftype rangeMaxUse = 1e-2f * _rng->max_distance_orient(ROTATION_PITCH_270) * (ftype)frontend->_useRngSwHgt;
967 0 : bool aboveUpperSwHgt = (terrainState - stateStruct.position.z) > rangeMaxUse;
968 0 : bool belowLowerSwHgt = (terrainState - stateStruct.position.z) < 0.7f * rangeMaxUse;
969 :
970 : // If the terrain height is consistent and we are moving slowly, then it can be
971 : // used as a height reference in combination with a range finder
972 : // apply a hysteresis to the speed check to prevent rapid switching
973 0 : ftype horizSpeed = stateStruct.velocity.xy().length();
974 0 : bool dontTrustTerrain = ((horizSpeed > frontend->_useRngSwSpd) && filterStatus.flags.horiz_vel) || !terrainHgtStable;
975 0 : ftype trust_spd_trigger = MAX((frontend->_useRngSwSpd - 1.0f),(frontend->_useRngSwSpd * 0.5f));
976 0 : bool trustTerrain = (horizSpeed < trust_spd_trigger) && terrainHgtStable;
977 :
978 : /*
979 : * Switch between range finder and primary height source using height above ground and speed thresholds with
980 : * hysteresis to avoid rapid switching. Using range finder for height requires a consistent terrain height
981 : * which cannot be assumed if the vehicle is moving horizontally.
982 : */
983 0 : if ((aboveUpperSwHgt || dontTrustTerrain) && (activeHgtSource == HGT_SOURCE_RNG)) {
984 : // cannot trust terrain or range finder so stop using range finder height
985 0 : if (frontend->_altSource == 0) {
986 0 : activeHgtSource = HGT_SOURCE_BARO;
987 0 : } else if (frontend->_altSource == 2) {
988 0 : activeHgtSource = HGT_SOURCE_GPS;
989 : }
990 0 : } else if (belowLowerSwHgt && trustTerrain && (prevTnb.c.z >= 0.7f)) {
991 : // reliable terrain and range finder so start using range finder height
992 0 : activeHgtSource = HGT_SOURCE_RNG;
993 : }
994 : #endif // AP_RANGEFINDER_ENABLED
995 3230372 : } else if (frontend->_altSource == 0) {
996 3230372 : activeHgtSource = HGT_SOURCE_BARO;
997 0 : } else if ((frontend->_altSource == 2) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) < 500) && validOrigin && gpsAccuracyGood) {
998 0 : activeHgtSource = HGT_SOURCE_GPS;
999 0 : } else if ((frontend->_altSource == 3) && validOrigin && rngBcnGoodToAlign) {
1000 0 : activeHgtSource = HGT_SOURCE_BCN;
1001 : }
1002 :
1003 : // Use Baro alt as a fallback if we lose range finder, GPS, external nav or Beacon
1004 3230372 : bool lostRngHgt = ((activeHgtSource == HGT_SOURCE_RNG) && (!rangeFinderDataIsFresh));
1005 3230372 : bool lostGpsHgt = ((activeHgtSource == HGT_SOURCE_GPS) && ((imuSampleTime_ms - lastTimeGpsReceived_ms) > 2000));
1006 3230372 : bool lostExtNavHgt = ((activeHgtSource == HGT_SOURCE_EXTNAV) && ((imuSampleTime_ms - extNavMeasTime_ms) > 2000));
1007 3230372 : bool lostRngBcnHgt = ((activeHgtSource == HGT_SOURCE_BCN) && ((imuSampleTime_ms - rngBcnDataDelayed.time_ms) > 2000));
1008 3230372 : if (lostRngHgt || lostGpsHgt || lostExtNavHgt || lostRngBcnHgt) {
1009 0 : activeHgtSource = HGT_SOURCE_BARO;
1010 : }
1011 :
1012 : // if there is new baro data to fuse, calculate filtered baro data required by other processes
1013 3230372 : if (baroDataToFuse) {
1014 : // calculate offset to baro data that enables us to switch to Baro height use during operation
1015 614058 : if (activeHgtSource != HGT_SOURCE_BARO) {
1016 0 : calcFiltBaroOffset();
1017 : }
1018 : // filtered baro data used to provide a reference for takeoff
1019 : // it is is reset to last height measurement on disarming in performArmingChecks()
1020 614058 : if (!dal.get_takeoff_expected()) {
1021 : const ftype gndHgtFiltTC = 0.5f;
1022 609244 : const ftype dtBaro = frontend->hgtAvg_ms*1.0e-3;
1023 609244 : ftype alpha = constrain_ftype(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
1024 609244 : meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
1025 : }
1026 : }
1027 :
1028 : // If we are not using GPS as the primary height sensor, correct EKF origin height so that
1029 : // combined local NED position height and origin height remains consistent with the GPS altitude
1030 : // This also enables the GPS height to be used as a backup height source
1031 3230372 : if (gpsDataToFuse &&
1032 287618 : (((frontend->_originHgtMode & (1 << 0)) && (activeHgtSource == HGT_SOURCE_BARO)) ||
1033 287618 : ((frontend->_originHgtMode & (1 << 1)) && (activeHgtSource == HGT_SOURCE_RNG)))
1034 : ) {
1035 0 : correctEkfOriginHeight();
1036 : }
1037 :
1038 : // Select the height measurement source
1039 3230372 : if (extNavDataToFuse && (activeHgtSource == HGT_SOURCE_EXTNAV)) {
1040 0 : hgtMea = -extNavDataDelayed.pos.z;
1041 0 : posDownObsNoise = sq(constrain_ftype(extNavDataDelayed.posErr, 0.01f, 10.0f));
1042 3230372 : } else if (rangeDataToFuse && (activeHgtSource == HGT_SOURCE_RNG)) {
1043 : // using range finder data
1044 : // correct for tilt using a flat earth model
1045 0 : if (prevTnb.c.z >= 0.7) {
1046 : // calculate height above ground
1047 0 : hgtMea = MAX(rangeDataDelayed.rng * prevTnb.c.z, rngOnGnd);
1048 : // correct for terrain position relative to datum
1049 0 : hgtMea -= terrainState;
1050 : // enable fusion
1051 0 : fuseHgtData = true;
1052 0 : velPosObs[5] = -hgtMea;
1053 : // set the observation noise
1054 0 : posDownObsNoise = sq(constrain_ftype(frontend->_rngNoise, 0.1f, 10.0f));
1055 : // add uncertainty created by terrain gradient and vehicle tilt
1056 0 : posDownObsNoise += sq(rangeDataDelayed.rng * frontend->_terrGradMax) * MAX(0.0f , (1.0f - sq(prevTnb.c.z)));
1057 : } else {
1058 : // disable fusion if tilted too far
1059 0 : fuseHgtData = false;
1060 : }
1061 3230372 : } else if (gpsDataToFuse && (activeHgtSource == HGT_SOURCE_GPS)) {
1062 : // using GPS data
1063 0 : hgtMea = gpsDataDelayed.hgt;
1064 : // enable fusion
1065 0 : velPosObs[5] = -hgtMea;
1066 0 : fuseHgtData = true;
1067 : // set the observation noise using receiver reported accuracy or the horizontal noise scaled for typical VDOP/HDOP ratio
1068 0 : if (gpsHgtAccuracy > 0.0f) {
1069 0 : posDownObsNoise = sq(constrain_ftype(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise, 100.0f));
1070 : } else {
1071 0 : posDownObsNoise = sq(constrain_ftype(1.5f * frontend->_gpsHorizPosNoise, 0.1f, 10.0f));
1072 : }
1073 3230372 : } else if (baroDataToFuse && (activeHgtSource == HGT_SOURCE_BARO)) {
1074 : // using Baro data
1075 614058 : hgtMea = baroDataDelayed.hgt - baroHgtOffset;
1076 : // enable fusion
1077 614058 : velPosObs[5] = -hgtMea;
1078 614058 : fuseHgtData = true;
1079 : // set the observation noise
1080 614058 : posDownObsNoise = sq(constrain_ftype(frontend->_baroAltNoise, 0.1f, 10.0f));
1081 : // reduce weighting (increase observation noise) on baro if we are likely to be in ground effect
1082 614058 : if (dal.get_takeoff_expected() || dal.get_touchdown_expected()) {
1083 6456 : posDownObsNoise *= frontend->gndEffectBaroScaler;
1084 : }
1085 : // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
1086 : // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
1087 614058 : if (motorsArmed && dal.get_takeoff_expected() && !assume_zero_sideslip()) {
1088 1224 : hgtMea = MAX(hgtMea, meaHgtAtTakeOff);
1089 : }
1090 : } else {
1091 2616314 : fuseHgtData = false;
1092 : }
1093 :
1094 : // If we haven't fused height data for a while, then declare the height data as being timed out
1095 : // set timeout period based on whether we have vertical GPS velocity available to constrain drift
1096 3230372 : hgtRetryTime_ms = ((useGpsVertVel || useExtNavVel) && !velTimeout) ? frontend->hgtRetryTimeMode0_ms : frontend->hgtRetryTimeMode12_ms;
1097 3230372 : if (imuSampleTime_ms - lastHgtPassTime_ms > hgtRetryTime_ms) {
1098 712 : hgtTimeout = true;
1099 : } else {
1100 3229660 : hgtTimeout = false;
1101 : }
1102 3230372 : }
1103 :
|
