Line data Source code
1 : #include "AP_NavEKF3_core.h"
2 :
3 : #include "AP_NavEKF3.h"
4 : #include "AP_NavEKF3_feature.h"
5 :
6 : #include <GCS_MAVLink/GCS.h>
7 : #include <AP_Logger/AP_Logger.h>
8 : #include <AP_DAL/AP_DAL.h>
9 : #include <AP_InternalError/AP_InternalError.h>
10 :
11 : #if AP_RANGEFINDER_ENABLED
12 : /********************************************************
13 : * OPT FLOW AND RANGE FINDER *
14 : ********************************************************/
15 :
16 : // Read the range finder and take new measurements if available
17 : // Apply a median filter
18 18761238 : void NavEKF3_core::readRangeFinder(void)
19 : {
20 : uint8_t midIndex;
21 : uint8_t maxIndex;
22 : uint8_t minIndex;
23 : // get theoretical correct range when the vehicle is on the ground
24 : // don't allow range to go below 5cm because this can cause problems with optical flow processing
25 18761238 : const auto *_rng = dal.rangefinder();
26 18761238 : if (_rng == nullptr) {
27 : return;
28 : }
29 1802100 : rngOnGnd = MAX(_rng->ground_clearance_orient(ROTATION_PITCH_270), 0.05f);
30 :
31 : // limit update rate to maximum allowed by data buffers
32 1802100 : if ((imuSampleTime_ms - lastRngMeasTime_ms) > frontend->sensorIntervalMin_ms) {
33 :
34 : // reset the timer used to control the measurement rate
35 387544 : lastRngMeasTime_ms = imuSampleTime_ms;
36 :
37 : // store samples and sample time into a ring buffer if valid
38 : // use data from two range finders if available
39 :
40 1162632 : for (uint8_t sensorIndex = 0; sensorIndex < ARRAY_SIZE(rngMeasIndex); sensorIndex++) {
41 775088 : const auto *sensor = _rng->get_backend(sensorIndex);
42 775088 : if (sensor == nullptr) {
43 556878 : continue;
44 : }
45 410917 : if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == AP_DAL_RangeFinder::Status::Good)) {
46 218210 : rngMeasIndex[sensorIndex] ++;
47 218210 : if (rngMeasIndex[sensorIndex] > 2) {
48 72679 : rngMeasIndex[sensorIndex] = 0;
49 : }
50 218210 : storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
51 218210 : storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance();
52 : } else {
53 192707 : continue;
54 : }
55 :
56 : // check for three fresh samples
57 218210 : bool sampleFresh[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3] = {};
58 872840 : for (uint8_t index = 0; index <= 2; index++) {
59 654630 : sampleFresh[sensorIndex][index] = (imuSampleTime_ms - storedRngMeasTime_ms[sensorIndex][index]) < 500;
60 : }
61 :
62 : // find the median value if we have three fresh samples
63 218210 : if (sampleFresh[sensorIndex][0] && sampleFresh[sensorIndex][1] && sampleFresh[sensorIndex][2]) {
64 217726 : if (storedRngMeas[sensorIndex][0] > storedRngMeas[sensorIndex][1]) {
65 : minIndex = 1;
66 : maxIndex = 0;
67 : } else {
68 : minIndex = 0;
69 : maxIndex = 1;
70 : }
71 217726 : if (storedRngMeas[sensorIndex][2] > storedRngMeas[sensorIndex][maxIndex]) {
72 : midIndex = maxIndex;
73 180411 : } else if (storedRngMeas[sensorIndex][2] < storedRngMeas[sensorIndex][minIndex]) {
74 : midIndex = minIndex;
75 : } else {
76 : midIndex = 2;
77 : }
78 :
79 : // don't allow time to go backwards
80 217726 : if (storedRngMeasTime_ms[sensorIndex][midIndex] > rangeDataNew.time_ms) {
81 132551 : rangeDataNew.time_ms = storedRngMeasTime_ms[sensorIndex][midIndex];
82 : }
83 :
84 : // limit the measured range to be no less than the on-ground range
85 217726 : rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd);
86 :
87 : // get position in body frame for the current sensor
88 217726 : rangeDataNew.sensor_idx = sensorIndex;
89 :
90 : // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
91 217726 : storedRange.push(rangeDataNew);
92 :
93 : // indicate we have updated the measurement
94 217726 : rngValidMeaTime_ms = imuSampleTime_ms;
95 :
96 484 : } else if (onGround && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) {
97 : // before takeoff we assume on-ground range value if there is no data
98 26 : rangeDataNew.time_ms = imuSampleTime_ms;
99 26 : rangeDataNew.rng = rngOnGnd;
100 :
101 : // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
102 26 : storedRange.push(rangeDataNew);
103 :
104 : // indicate we have updated the measurement
105 26 : rngValidMeaTime_ms = imuSampleTime_ms;
106 :
107 : }
108 : }
109 : }
110 : }
111 : #endif // AP_RANGEFINDER_ENABLED
112 :
113 7162 : void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, uint16_t delay_ms, const Vector3f &posOffset)
114 : {
115 : #if EK3_FEATURE_BODY_ODOM
116 : // protect against NaN
117 7162 : if (isnan(quality) || delPos.is_nan() || delAng.is_nan() || isnan(delTime) || posOffset.is_nan()) {
118 0 : return;
119 : }
120 :
121 : // limit update rate to maximum allowed by sensor buffers and fusion process
122 : // don't try to write to buffer until the filter has been initialised
123 7162 : if (((timeStamp_ms - bodyOdmMeasTime_ms) < frontend->sensorIntervalMin_ms) || (delTime < dtEkfAvg) || !statesInitialised) {
124 : return;
125 : }
126 :
127 : // subtract delay from timestamp
128 4170 : timeStamp_ms -= delay_ms;
129 :
130 4170 : bodyOdmDataNew.body_offset = posOffset.toftype();
131 4170 : bodyOdmDataNew.vel = delPos.toftype() * (1.0/delTime);
132 4170 : bodyOdmDataNew.time_ms = timeStamp_ms;
133 4170 : bodyOdmDataNew.angRate = (delAng * (1.0/delTime)).toftype();
134 4170 : bodyOdmMeasTime_ms = timeStamp_ms;
135 :
136 : // simple model of accuracy
137 : // TODO move this calculation outside of EKF into the sensor driver
138 4170 : bodyOdmDataNew.velErr = frontend->_visOdmVelErrMin + (frontend->_visOdmVelErrMax - frontend->_visOdmVelErrMin) * (1.0f - 0.01f * quality);
139 :
140 4170 : storedBodyOdm.push(bodyOdmDataNew);
141 : #endif // EK3_FEATURE_BODY_ODOM
142 : }
143 :
144 8306 : void NavEKF3_core::writeWheelOdom(float delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset, float radius)
145 : {
146 : #if EK3_FEATURE_BODY_ODOM
147 : // This is a simple hack to get wheel encoder data into the EKF and verify the interface sign conventions and units
148 : // It uses the exisiting body frame velocity fusion.
149 : // TODO implement a dedicated wheel odometry observation model
150 :
151 : // rate limiting to 50hz should be done by the caller
152 : // limit update rate to maximum allowed by sensor buffers and fusion process
153 : // don't try to write to buffer until the filter has been initialised
154 8306 : if ((delTime < dtEkfAvg) || !statesInitialised) {
155 2656 : return;
156 : }
157 :
158 5650 : wheel_odm_elements wheelOdmDataNew = {};
159 5650 : wheelOdmDataNew.hub_offset = posOffset.toftype();
160 5650 : wheelOdmDataNew.delAng = delAng;
161 5650 : wheelOdmDataNew.radius = radius;
162 5650 : wheelOdmDataNew.delTime = delTime;
163 :
164 : // because we are currently converting to an equivalent velocity measurement before fusing
165 : // the measurement time is moved back to the middle of the sampling period
166 5650 : wheelOdmDataNew.time_ms = timeStamp_ms - (uint32_t)(500.0f * delTime);
167 :
168 5650 : storedWheelOdm.push(wheelOdmDataNew);
169 : #endif // EK3_FEATURE_BODY_ODOM
170 : }
171 :
172 : #if EK3_FEATURE_OPTFLOW_FUSION
173 : // write the raw optical flow measurements
174 : // this needs to be called externally.
175 19976 : void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
176 : {
177 : // limit update rate to maximum allowed by sensor buffers
178 19976 : if ((imuSampleTime_ms - flowMeaTime_ms) < frontend->sensorIntervalMin_ms) {
179 : return;
180 : }
181 :
182 : // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
183 : // The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
184 : // A positive X rate is produced by a positive sensor rotation about the X axis
185 : // A positive Y rate is produced by a positive sensor rotation about the Y axis
186 : // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
187 : // negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
188 19976 : flowMeaTime_ms = imuSampleTime_ms;
189 : // calculate bias errors on flow sensor gyro rates, but protect against spikes in data
190 : // reset the accumulated body delta angle and time
191 : // don't do the calculation if not enough time lapsed for a reliable body rate measurement
192 19976 : if (delTimeOF > 0.01f) {
193 19668 : flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_ftype((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
194 19668 : flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_ftype((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
195 19668 : delAngBodyOF.zero();
196 19668 : delTimeOF = 0.0f;
197 : }
198 : // by definition if this function is called, then flow measurements have been provided so we
199 : // need to run the optical flow takeoff detection
200 19976 : detectOptFlowTakeoff();
201 :
202 : // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
203 19976 : if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
204 : // correct flow sensor body rates for bias and write
205 19976 : of_elements ofDataNew {};
206 19976 : ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
207 19976 : ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
208 : // the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
209 19976 : if (delTimeOF > 0.001f) {
210 : // first preference is to use the rate averaged over the same sampling period as the flow sensor
211 8 : ofDataNew.bodyRadXYZ.z = delAngBodyOF.z / delTimeOF;
212 19968 : } else if (imuDataNew.delAngDT > 0.001f){
213 : // second preference is to use most recent IMU data
214 19968 : ofDataNew.bodyRadXYZ.z = imuDataNew.delAng.z / imuDataNew.delAngDT;
215 : } else {
216 : // third preference is use zero
217 0 : ofDataNew.bodyRadXYZ.z = 0.0f;
218 : }
219 : // write uncorrected flow rate measurements
220 : // note correction for different axis and sign conventions used by the px4flow sensor
221 19976 : ofDataNew.flowRadXY = - rawFlowRates.toftype(); // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
222 : // write the flow sensor position in body frame
223 19976 : ofDataNew.body_offset = posOffset.toftype();
224 : // write the flow sensor height override
225 19976 : ofDataNew.heightOverride = heightOverride;
226 : // write flow rate measurements corrected for body rates
227 19976 : ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
228 19976 : ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
229 : // record time last observation was received so we can detect loss of data elsewhere
230 19976 : flowValidMeaTime_ms = imuSampleTime_ms;
231 : // estimate sample time of the measurement
232 19976 : ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
233 : // Correct for the average intersampling delay due to the filter updaterate
234 19976 : ofDataNew.time_ms -= localFilterTimeStep_ms/2;
235 : // Prevent time delay exceeding age of oldest IMU data in the buffer
236 19976 : ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms);
237 : // Save data to buffer
238 19976 : storedOF.push(ofDataNew);
239 : }
240 : }
241 : #endif // EK3_FEATURE_OPTFLOW_FUSION
242 :
243 :
244 : /********************************************************
245 : * MAGNETOMETER *
246 : ********************************************************/
247 :
248 : // try changing compass, return true if a new compass is found
249 5315 : void NavEKF3_core::tryChangeCompass(void)
250 : {
251 5315 : const auto &compass = dal.compass();
252 5315 : const uint8_t maxCount = compass.get_count();
253 :
254 : // search through the list of magnetometers
255 15945 : for (uint8_t i=1; i<maxCount; i++) {
256 10630 : uint8_t tempIndex = magSelectIndex + i;
257 : // loop back to the start index if we have exceeded the bounds
258 10630 : if (tempIndex >= maxCount) {
259 19 : tempIndex -= maxCount;
260 : }
261 10630 : tryChangeCompass(tempIndex);
262 : }
263 5315 : }
264 :
265 : // try changing to a specific compass index
266 10645 : void NavEKF3_core::tryChangeCompass(uint8_t mag_index)
267 : {
268 10645 : const auto &compass = dal.compass();
269 10645 : const uint8_t maxCount = compass.get_count();
270 :
271 10645 : if (mag_index >= maxCount) {
272 : // we assume the passed index should be valid
273 : return;
274 : }
275 :
276 : // if the magnetometer is allowed to be used for yaw and has a different index, we start using it
277 10645 : if (compass.healthy(mag_index) && compass.use_for_yaw(mag_index) && mag_index != magSelectIndex) {
278 35 : magSelectIndex = mag_index;
279 35 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF3 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
280 : // reset the timeout flag and timer
281 35 : magTimeout = false;
282 35 : lastHealthyMagTime_ms = imuSampleTime_ms;
283 : // zero the learned magnetometer bias states
284 35 : stateStruct.body_magfield.zero();
285 : // clear the measurement buffer
286 35 : storedMag.reset();
287 : // clear the data waiting flag so that we do not use any data pending from the previous sensor
288 35 : magDataToFuse = false;
289 : // request a reset of the magnetic field states
290 35 : magStateResetRequest = true;
291 : // declare the field unlearned so that the reset request will be obeyed
292 35 : magFieldLearned = false;
293 : // reset body mag variances on next CovariancePrediction
294 35 : needMagBodyVarReset = true;
295 35 : return;
296 : }
297 : }
298 :
299 : // check for new magnetometer data and update store measurements if available
300 20784458 : void NavEKF3_core::readMagData()
301 : {
302 20784458 : const auto &compass = dal.compass();
303 :
304 20784458 : if (!compass.available()) {
305 604 : allMagSensorsFailed = true;
306 604 : return;
307 : }
308 :
309 : // If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
310 : // magnetometer, then declare the magnetometers as failed for this flight
311 20783854 : const uint8_t maxCount = compass.get_count();
312 20783854 : if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
313 0 : allMagSensorsFailed = true;
314 0 : return;
315 : }
316 :
317 20783854 : if (compass.learn_offsets_enabled()) {
318 2 : wasLearningCompass_ms = imuSampleTime_ms;
319 20783852 : } else if (wasLearningCompass_ms != 0 && imuSampleTime_ms - wasLearningCompass_ms > 1000) {
320 : // allow time for old data to clear the buffer before signalling other code that compass data can be used
321 0 : wasLearningCompass_ms = 0;
322 : }
323 :
324 : // If the magnetometer has timed out (been rejected for too long), we find another magnetometer to use if available
325 : // Don't do this if we are on the ground because there can be magnetic interference and we need to know if there is a problem
326 : // before taking off. Don't do this within the first 30 seconds from startup because the yaw error could be due to large yaw gyro bias affsets
327 : // if the timeout is due to a sensor failure, then declare a timeout regardless of onground status
328 20783854 : if (maxCount > 1) {
329 20783854 : bool fusionTimeout = magTimeout && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000 && !(frontend->_affinity & EKF_AFFINITY_MAG);
330 20783854 : bool sensorTimeout = !compass.healthy(magSelectIndex) && imuSampleTime_ms - lastMagRead_ms > frontend->magFailTimeLimit_ms;
331 20783854 : if (fusionTimeout || sensorTimeout) {
332 5315 : tryChangeCompass();
333 : }
334 : }
335 :
336 : // limit compass update rate to prevent high processor loading because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
337 20783854 : if (use_compass() &&
338 20783854 : compass.healthy(magSelectIndex) &&
339 19846541 : ((compass.last_update_usec(magSelectIndex) - lastMagUpdate_us) > 1000 * frontend->sensorIntervalMin_ms)) {
340 :
341 : // detect changes to magnetometer offset parameters and reset states
342 2794118 : Vector3F nowMagOffsets = compass.get_offsets(magSelectIndex).toftype();
343 2794118 : bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
344 : if (changeDetected) {
345 : // zero the learned magnetometer bias states
346 67 : stateStruct.body_magfield.zero();
347 : // clear the measurement buffer
348 67 : storedMag.reset();
349 :
350 : // reset body mag variances on next
351 : // CovariancePrediction. This copes with possible errors
352 : // in the new offsets
353 67 : needMagBodyVarReset = true;
354 : }
355 2794118 : lastMagOffsets = nowMagOffsets;
356 2794118 : lastMagOffsetsValid = true;
357 :
358 : // store time of last measurement update
359 2794118 : lastMagUpdate_us = compass.last_update_usec(magSelectIndex);
360 :
361 : // Magnetometer data at the current time horizon
362 2794118 : mag_elements magDataNew;
363 :
364 : // estimate of time magnetometer measurement was taken, allowing for delays
365 2794118 : magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
366 :
367 : // Correct for the average intersampling delay due to the filter updaterate
368 2794118 : magDataNew.time_ms -= localFilterTimeStep_ms/2;
369 :
370 : // read compass data and scale to improve numerical conditioning
371 2794118 : magDataNew.mag = (compass.get_field(magSelectIndex) * 0.001f).toftype();
372 :
373 : // check for consistent data between magnetometers
374 2794118 : consistentMagData = compass.consistent();
375 :
376 : // save magnetometer measurement to buffer to be fused later
377 2794118 : storedMag.push(magDataNew);
378 :
379 : // remember time we read compass, to detect compass sensor failure
380 2794118 : lastMagRead_ms = imuSampleTime_ms;
381 : }
382 : }
383 :
384 : /********************************************************
385 : * Inertial Measurements *
386 : ********************************************************/
387 :
388 : /*
389 : * Read IMU delta angle and delta velocity measurements and downsample to 100Hz
390 : * for storage in the data buffers used by the EKF. If the IMU data arrives at
391 : * lower rate than 100Hz, then no downsampling or upsampling will be performed.
392 : * Downsampling is done using a method that does not introduce coning or sculling
393 : * errors.
394 : */
395 79223583 : void NavEKF3_core::readIMUData(bool startPredictEnabled)
396 : {
397 79223583 : const auto &ins = dal.ins();
398 :
399 : // calculate an averaged IMU update rate using a spike and lowpass filter combination
400 79223583 : dtIMUavg = 0.02f * constrain_ftype(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;
401 :
402 : // the imu sample time is used as a common time reference throughout the filter
403 79223583 : imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
404 :
405 : uint8_t accel_active, gyro_active;
406 :
407 79223583 : if (ins.use_accel(imu_index)) {
408 : accel_active = imu_index;
409 : } else {
410 0 : accel_active = ins.get_first_usable_accel();
411 : }
412 :
413 79223583 : if (ins.use_gyro(imu_index)) {
414 : gyro_active = imu_index;
415 : } else {
416 0 : gyro_active = ins.get_first_usable_gyro();
417 : }
418 :
419 79223583 : if (gyro_active != gyro_index_active) {
420 : // we are switching active gyro at runtime. Copy over the
421 : // bias we have learned from the previously inactive
422 : // gyro. We don't re-init the bias uncertainty as it should
423 : // have the same uncertainty as the previously active gyro
424 0 : stateStruct.gyro_bias = inactiveBias[gyro_active].gyro_bias;
425 0 : gyro_index_active = gyro_active;
426 : }
427 :
428 79223583 : if (accel_active != accel_index_active) {
429 : // switch to the learned accel bias for this IMU
430 0 : stateStruct.accel_bias = inactiveBias[accel_active].accel_bias;
431 0 : accel_index_active = accel_active;
432 : }
433 :
434 : // update the inactive bias states
435 79223583 : learnInactiveBiases();
436 :
437 : // run movement check using IMU data
438 79223583 : updateMovementCheck();
439 :
440 79223583 : readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT);
441 79223583 : accelPosOffset = ins.get_imu_pos_offset(accel_index_active).toftype();
442 79223583 : imuDataNew.accel_index = accel_index_active;
443 :
444 : // Get delta angle data from primary gyro or primary if not available
445 79223583 : readDeltaAngle(gyro_index_active, imuDataNew.delAng, imuDataNew.delAngDT);
446 79223583 : imuDataNew.delAngDT = MAX(imuDataNew.delAngDT, 1.0e-4f);
447 79223583 : imuDataNew.gyro_index = gyro_index_active;
448 :
449 : // Get current time stamp
450 79223583 : imuDataNew.time_ms = imuSampleTime_ms;
451 :
452 : // Accumulate the measurement time interval for the delta velocity and angle data
453 79223583 : imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
454 79223583 : imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;
455 :
456 : // use the most recent IMU index for the downsampled IMU
457 : // data. This isn't strictly correct if we switch IMUs between
458 : // samples
459 79223583 : imuDataDownSampledNew.gyro_index = imuDataNew.gyro_index;
460 79223583 : imuDataDownSampledNew.accel_index = imuDataNew.accel_index;
461 :
462 : // Rotate quaternon atitude from previous to new and normalise.
463 : // Accumulation using quaternions prevents introduction of coning errors due to downsampling
464 79223583 : imuQuatDownSampleNew.rotate(imuDataNew.delAng);
465 79223583 : imuQuatDownSampleNew.normalize();
466 :
467 : // Rotate the latest delta velocity into body frame at the start of accumulation
468 79223583 : Matrix3F deltaRotMat;
469 79223583 : imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
470 :
471 : // Apply the delta velocity to the delta velocity accumulator
472 79223583 : imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel;
473 :
474 : // Keep track of the number of IMU frames since the last state prediction
475 79223583 : framesSincePredict++;
476 :
477 : /*
478 : * If the target EKF time step has been accumulated, and the frontend has allowed start of a new predict cycle,
479 : * then store the accumulated IMU data to be used by the state prediction, ignoring the frontend permission if more
480 : * than twice the target time has lapsed. Adjust the target EKF step time threshold to allow for timing jitter in the
481 : * IMU data.
482 : */
483 79223583 : if ((imuDataDownSampledNew.delAngDT >= (EKF_TARGET_DT-(dtIMUavg*0.5f)) && startPredictEnabled) ||
484 : (imuDataDownSampledNew.delAngDT >= 2.0f*EKF_TARGET_DT)) {
485 :
486 : // convert the accumulated quaternion to an equivalent delta angle
487 20078569 : imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);
488 :
489 : // Time stamp the data
490 20078569 : imuDataDownSampledNew.time_ms = imuSampleTime_ms;
491 :
492 : // Write data to the FIFO IMU buffer
493 20078569 : storedIMU.push_youngest_element(imuDataDownSampledNew);
494 :
495 : // calculate the achieved average time step rate for the EKF using a combination spike and LPF
496 20078569 : ftype dtNow = constrain_ftype(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
497 20078569 : dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;
498 :
499 : // do an addtional down sampling for data used to sample XY body frame drag specific forces
500 20078569 : SampleDragData(imuDataDownSampledNew);
501 :
502 : // zero the accumulated IMU data and quaternion
503 20078569 : imuDataDownSampledNew.delAng.zero();
504 20078569 : imuDataDownSampledNew.delVel.zero();
505 20078569 : imuDataDownSampledNew.delAngDT = 0.0f;
506 20078569 : imuDataDownSampledNew.delVelDT = 0.0f;
507 20078569 : imuQuatDownSampleNew[0] = 1.0f;
508 20078569 : imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f;
509 :
510 : // reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle
511 20078569 : framesSincePredict = 0;
512 :
513 : // set the flag to let the filter know it has new IMU data and needs to run
514 20078569 : runUpdates = true;
515 :
516 : // extract the oldest available data from the FIFO buffer
517 20078569 : imuDataDelayed = storedIMU.get_oldest_element();
518 :
519 : // protect against delta time going to zero
520 20078569 : ftype minDT = 0.1f * dtEkfAvg;
521 20078569 : imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
522 20078569 : imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);
523 :
524 20078569 : updateTimingStatistics();
525 :
526 : // correct the extracted IMU data for sensor errors
527 20078569 : delAngCorrected = imuDataDelayed.delAng;
528 20078569 : delVelCorrected = imuDataDelayed.delVel;
529 20078569 : correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT, imuDataDelayed.gyro_index);
530 20078569 : correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT, imuDataDelayed.accel_index);
531 :
532 20078569 : } else {
533 : // we don't have new IMU data in the buffer so don't run filter updates on this time step
534 59145014 : runUpdates = false;
535 : }
536 79223583 : }
537 :
538 : // read the delta velocity and corresponding time interval from the IMU
539 : // return false if data is not available
540 79223583 : bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt) {
541 79223583 : const auto &ins = dal.ins();
542 :
543 79223583 : if (ins_index < ins.get_accel_count()) {
544 79223583 : Vector3f dVelF;
545 : float dVel_dtF;
546 79223583 : ins.get_delta_velocity(ins_index, dVelF, dVel_dtF);
547 79223583 : dVel = dVelF.toftype();
548 79223583 : dVel_dt = dVel_dtF;
549 79223583 : dVel_dt = MAX(dVel_dt,1.0e-4);
550 : return true;
551 : }
552 : return false;
553 : }
554 :
555 : /********************************************************
556 : * Global Position Measurement *
557 : ********************************************************/
558 :
559 : // check for new valid GPS data and update stored measurement if available
560 19666871 : void NavEKF3_core::readGpsData()
561 : {
562 : // check for new GPS data
563 19666871 : const auto &gps = dal.gps();
564 :
565 : // limit update rate to avoid overflowing the FIFO buffer
566 19666871 : if (gps.last_message_time_ms(selected_gps) - lastTimeGpsReceived_ms <= frontend->sensorIntervalMin_ms) {
567 18320425 : return;
568 : }
569 :
570 1555411 : if (gps.status(selected_gps) < AP_GPS_FixType::FIX_3D) {
571 : // report GPS fix status
572 208965 : gpsCheckStatus.bad_fix = true;
573 208965 : dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
574 208965 : return;
575 : }
576 :
577 : // report GPS fix status
578 1346446 : gpsCheckStatus.bad_fix = false;
579 :
580 : // store fix time from previous read
581 : const uint32_t secondLastGpsTime_ms = lastTimeGpsReceived_ms;
582 :
583 : // get current fix time
584 1346446 : lastTimeGpsReceived_ms = gps.last_message_time_ms(selected_gps);
585 :
586 : // estimate when the GPS fix was valid, allowing for GPS processing and other delays
587 : // ideally we should be using a timing signal from the GPS receiver to set this time
588 : // Use the driver specified delay
589 1346446 : float gps_delay_sec = 0;
590 1346446 : gps.get_lag(selected_gps, gps_delay_sec);
591 1346446 : gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(gps_delay_sec * 1000.0f);
592 :
593 : // Correct for the average intersampling delay due to the filter updaterate
594 1346446 : gpsDataNew.time_ms -= localFilterTimeStep_ms/2;
595 :
596 : // Prevent the time stamp falling outside the oldest and newest IMU data in the buffer
597 1346446 : gpsDataNew.time_ms = MIN(MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms),imuDataDownSampledNew.time_ms);
598 :
599 : // Get which GPS we are using for position information
600 1346446 : gpsDataNew.sensor_idx = selected_gps;
601 :
602 : // read the NED velocity from the GPS
603 1346446 : gpsDataNew.vel = gps.velocity(selected_gps).toftype();
604 1346446 : gpsDataNew.have_vz = gps.have_vertical_velocity(selected_gps);
605 :
606 : // position and velocity are not yet corrected for sensor position
607 1346446 : gpsDataNew.corrected = false;
608 :
609 : // Use the speed and position accuracy from the GPS if available, otherwise set it to zero.
610 : // Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data
611 1346446 : ftype alpha = constrain_ftype(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
612 1346446 : gpsSpdAccuracy *= (1.0f - alpha);
613 : float gpsSpdAccRaw;
614 1346446 : if (!gps.speed_accuracy(selected_gps, gpsSpdAccRaw)) {
615 2943 : gpsSpdAccuracy = 0.0f;
616 : } else {
617 1343503 : gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
618 1343503 : gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f);
619 1343503 : gpsSpdAccuracy = MAX(gpsSpdAccuracy,frontend->_gpsHorizVelNoise);
620 : }
621 1346446 : gpsPosAccuracy *= (1.0f - alpha);
622 : float gpsPosAccRaw;
623 1346446 : if (!gps.horizontal_accuracy(selected_gps, gpsPosAccRaw)) {
624 786 : gpsPosAccuracy = 0.0f;
625 : } else {
626 1345660 : gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw);
627 1345660 : gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f);
628 1345660 : gpsPosAccuracy = MAX(gpsPosAccuracy, frontend->_gpsHorizPosNoise);
629 : }
630 1346446 : gpsHgtAccuracy *= (1.0f - alpha);
631 : float gpsHgtAccRaw;
632 1346446 : if (!gps.vertical_accuracy(selected_gps, gpsHgtAccRaw)) {
633 786 : gpsHgtAccuracy = 0.0f;
634 : } else {
635 1345660 : gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw);
636 1345660 : gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f);
637 1345660 : gpsHgtAccuracy = MAX(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise);
638 : }
639 :
640 : // check if we have enough GPS satellites and increase the gps noise scaler if we don't
641 1346446 : if (gps.num_sats(selected_gps) >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
642 1124882 : gpsNoiseScaler = 1.0f;
643 221564 : } else if (gps.num_sats(selected_gps) == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
644 0 : gpsNoiseScaler = 1.4f;
645 : } else { // <= 4 satellites or in constant position mode
646 221564 : gpsNoiseScaler = 2.0f;
647 : }
648 :
649 : // Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly
650 1346446 : if (gpsDataNew.have_vz && frontend->sources.useVelZSource(AP_NavEKF_Source::SourceZ::GPS, core_index)) {
651 1295891 : useGpsVertVel = true;
652 : } else {
653 50555 : useGpsVertVel = false;
654 : }
655 :
656 1346446 : if (frontend->option_is_enabled(NavEKF3::Option::JammingExpected) &&
657 5742 : (lastTimeGpsReceived_ms - secondLastGpsTime_ms) > frontend->gpsNoFixTimeout_ms) {
658 16 : const bool doingBodyVelNav = (imuSampleTime_ms - prevBodyVelFuseTime_ms < 1000);
659 16 : const bool doingFlowNav = (imuSampleTime_ms - prevFlowFuseTime_ms < 1000);;
660 16 : const bool canDoWindRelNav = assume_zero_sideslip();
661 16 : const bool canDeadReckon = ((doingFlowNav && gndOffsetValid) || canDoWindRelNav || doingBodyVelNav);
662 : if (canDeadReckon) {
663 : // If we can do dead reckoning with a data source other than GPS there is time to wait
664 : // for GPS alignment checks to pass before using GPS inside the EKF.
665 : // this gets set back to false in calcGpsGoodToAlign() when GPS checks pass
666 16 : waitingForGpsChecks = true;
667 : // force GPS checks to restart
668 16 : lastPreAlignGpsCheckTime_ms = 0;
669 16 : lastGpsVelFail_ms = imuSampleTime_ms;
670 16 : lastGpsVelPass_ms = 0;
671 16 : gpsGoodToAlign = false;
672 : } else {
673 0 : waitingForGpsChecks = false;
674 : }
675 : }
676 :
677 : // Monitor quality of the GPS velocity data before and after alignment
678 1346446 : calcGpsGoodToAlign();
679 :
680 : // Post-alignment checks
681 1346446 : calcGpsGoodForFlight();
682 :
683 : // Read the GPS location in WGS-84 lat,long,height coordinates
684 1346446 : const Location &gpsloc = gps.location(selected_gps);
685 :
686 : // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
687 1346446 : if (gpsGoodToAlign && !validOrigin) {
688 1401 : Location gpsloc_fieldelevation = gpsloc;
689 : // if flying, correct for height change from takeoff so that the origin is at field elevation
690 1401 : if (inFlight) {
691 63 : gpsloc_fieldelevation.alt += (int32_t)(100.0f * stateStruct.position.z);
692 : }
693 :
694 1401 : if (!setOriginLLH(gpsloc_fieldelevation)) {
695 : // set an error as an attempt was made to set the origin more than once
696 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
697 0 : return;
698 : }
699 :
700 : // set the NE earth magnetic field states using the published declination
701 : // and set the corresponding variances and covariances
702 1401 : alignMagStateDeclination();
703 :
704 : // Set the height of the NED origin
705 1401 : ekfGpsRefHgt = (double)0.01 * (double)gpsloc.alt + (double)outputDataNew.position.z;
706 :
707 : // Set the uncertainty of the GPS origin height
708 1401 : ekfOriginHgtVar = sq(gpsHgtAccuracy);
709 :
710 : }
711 :
712 1346446 : if (gpsGoodToAlign && !have_table_earth_field) {
713 170 : setEarthFieldFromLocation(gpsloc);
714 : }
715 :
716 : // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
717 : // and are not waiting for GPs checks to pass
718 1346446 : if (validOrigin && !waitingForGpsChecks) {
719 1248420 : gpsDataNew.lat = gpsloc.lat;
720 1248420 : gpsDataNew.lng = gpsloc.lng;
721 1248420 : if ((frontend->_originHgtMode & (1<<2)) == 0) {
722 : // the height adjustment to match GPS is being achieved by adjusting the origin height
723 1232971 : gpsDataNew.hgt = (ftype)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
724 : } else {
725 : // the height adjustment to match GPS is being achieved by adjusting the measurements
726 15449 : gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
727 : }
728 1248420 : storedGPS.push(gpsDataNew);
729 : // declare GPS in use
730 1248420 : gpsIsInUse = true;
731 : }
732 : }
733 :
734 : // check for new valid GPS yaw data
735 19666871 : void NavEKF3_core::readGpsYawData()
736 : {
737 19666871 : const auto &gps = dal.gps();
738 :
739 : // if the GPS has yaw data then fuse it as an Euler yaw angle
740 : float yaw_deg, yaw_accuracy_deg;
741 : uint32_t yaw_time_ms;
742 17816285 : if (gps.status(selected_gps) >= AP_GPS_FixType::FIX_3D &&
743 19666871 : dal.gps().gps_yaw_deg(selected_gps, yaw_deg, yaw_accuracy_deg, yaw_time_ms) &&
744 84010 : yaw_time_ms != yawMeasTime_ms) {
745 : // GPS modules are rather too optimistic about their
746 : // accuracy. Set to min of 5 degrees here to prevent
747 : // the user constantly receiving warnings about high
748 : // normalised yaw innovations
749 10752 : const ftype min_yaw_accuracy_deg = 5.0f;
750 10752 : yaw_accuracy_deg = MAX(yaw_accuracy_deg, min_yaw_accuracy_deg);
751 : #if EK3_FEATURE_MOVING_BASELINE
752 10752 : writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), yaw_time_ms, 2, gps.get_mb_yaw_offset(selected_gps));
753 : #else
754 : writeEulerYawAngle(radians(yaw_deg), radians(yaw_accuracy_deg), yaw_time_ms, 2);
755 : #endif
756 : }
757 19666871 : }
758 :
759 : // read the delta angle and corresponding time interval from the IMU
760 : // return false if data is not available
761 79223583 : bool NavEKF3_core::readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAngDT) {
762 79223583 : const auto &ins = dal.ins();
763 :
764 79223583 : if (ins_index < ins.get_gyro_count()) {
765 79223583 : Vector3f dAngF;
766 : float dAngDTF;
767 79223583 : ins.get_delta_angle(ins_index, dAngF, dAngDTF);
768 79223583 : dAng = dAngF.toftype();
769 79223583 : dAngDT = dAngDTF;
770 : return true;
771 : }
772 : return false;
773 : }
774 :
775 :
776 : /********************************************************
777 : * Height Measurements *
778 : ********************************************************/
779 :
780 : // check for new pressure altitude measurement data and update stored measurement if available
781 19666871 : void NavEKF3_core::readBaroData()
782 : {
783 : // check to see if baro measurement has changed so we know if a new measurement has arrived
784 : // limit update rate to avoid overflowing the FIFO buffer
785 19666871 : const auto &baro = dal.baro();
786 19666871 : if (baro.get_last_update(selected_baro) - lastBaroReceived_ms > frontend->sensorIntervalMin_ms) {
787 :
788 2845431 : baroDataNew.hgt = baro.get_altitude(selected_baro);
789 :
790 : // time stamp used to check for new measurement
791 2845431 : lastBaroReceived_ms = baro.get_last_update(selected_baro);
792 :
793 : // estimate of time height measurement was taken, allowing for delays
794 2845431 : baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms;
795 :
796 : // Correct for the average intersampling delay due to the filter updaterate
797 2845431 : baroDataNew.time_ms -= localFilterTimeStep_ms/2;
798 :
799 : // Prevent time delay exceeding age of oldest IMU data in the buffer
800 2845431 : baroDataNew.time_ms = MAX(baroDataNew.time_ms,imuDataDelayed.time_ms);
801 :
802 : // save baro measurement to buffer to be fused later
803 2845431 : storedBaro.push(baroDataNew);
804 : }
805 19666871 : }
806 :
807 : // calculate filtered offset between baro height measurement and EKF height estimate
808 : // offset should be subtracted from baro measurement to match filter estimate
809 : // offset is used to enable reversion to baro from alternate height data source
810 9847 : void NavEKF3_core::calcFiltBaroOffset()
811 : {
812 : // Apply a first order LPF with spike protection
813 9847 : baroHgtOffset += 0.1f * constrain_ftype(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
814 9847 : }
815 :
816 : // correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
817 30882 : void NavEKF3_core::correctEkfOriginHeight()
818 : {
819 : // Estimate the WGS-84 height of the EKF's origin using a Bayes filter
820 :
821 : // calculate the variance of our a-priori estimate of the ekf origin height
822 30882 : ftype deltaTime = constrain_ftype(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0, 1.0);
823 30882 : if (activeHgtSource == AP_NavEKF_Source::SourceZ::BARO) {
824 : // Use the baro drift rate
825 : const ftype baroDriftRate = 0.05;
826 30882 : ekfOriginHgtVar += sq(baroDriftRate * deltaTime);
827 0 : } else if (activeHgtSource == AP_NavEKF_Source::SourceZ::RANGEFINDER) {
828 : // use the worse case expected terrain gradient and vehicle horizontal speed
829 : const ftype maxTerrGrad = 0.25;
830 0 : ekfOriginHgtVar += sq(maxTerrGrad * stateStruct.velocity.xy().length() * deltaTime);
831 : } else {
832 : // by definition our height source is absolute so cannot run this filter
833 : return;
834 : }
835 30882 : lastOriginHgtTime_ms = imuDataDelayed.time_ms;
836 :
837 : // calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error
838 : // when not using GPS as height source
839 30882 : ftype originHgtObsVar = sq(gpsHgtAccuracy) + P[9][9];
840 :
841 : // calculate the correction gain
842 30882 : ftype gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);
843 :
844 : // calculate the innovation
845 30882 : ftype innovation = - stateStruct.position.z - gpsDataDelayed.hgt;
846 :
847 : // check the innovation variance ratio
848 30882 : ftype ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar);
849 :
850 : // correct the EKF origin and variance estimate if the innovation is less than 5-sigma
851 30882 : if (ratio < 25.0f && gpsAccuracyGood) {
852 30882 : ekfGpsRefHgt -= (double)(gain * innovation);
853 30882 : ekfOriginHgtVar -= MAX(gain * ekfOriginHgtVar , 0.0f);
854 : }
855 : }
856 :
857 : /********************************************************
858 : * Air Speed Measurements *
859 : ********************************************************/
860 :
861 : // check for new airspeed data and update stored measurements if available
862 18761238 : void NavEKF3_core::readAirSpdData()
863 : {
864 18761238 : const float EAS2TAS = dal.get_EAS2TAS();
865 : // if airspeed reading is valid and is set by the user to be used and has been updated then
866 : // we take a new reading, convert from EAS to TAS and set the flag letting other functions
867 : // know a new measurement is available
868 :
869 18761238 : if (useAirspeed()) {
870 5612455 : const auto *airspeed = dal.airspeed();
871 5612455 : if (airspeed &&
872 5612455 : (airspeed->last_update_ms(selected_airspeed) - timeTasReceived_ms) > frontend->sensorIntervalMin_ms) {
873 953700 : tasDataNew.allowFusion = airspeed->healthy(selected_airspeed) && airspeed->use(selected_airspeed);
874 953700 : if (tasDataNew.allowFusion) {
875 953700 : tasDataNew.tas = airspeed->get_airspeed(selected_airspeed) * EAS2TAS;
876 953700 : timeTasReceived_ms = airspeed->last_update_ms(selected_airspeed);
877 953700 : tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
878 953700 : tasDataNew.tasVariance = sq(MAX(frontend->_easNoise * EAS2TAS, 0.5f));
879 : // Correct for the average intersampling delay due to the filter update rate
880 953700 : tasDataNew.time_ms -= localFilterTimeStep_ms/2;
881 : // Save data into the buffer to be fused when the fusion time horizon catches up with it
882 953700 : storedTAS.push(tasDataNew);
883 : }
884 : }
885 : // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
886 5612455 : tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
887 13148783 : } else if (assume_zero_sideslip()) {
888 : // synthetic airspeed is only valid for 'fly forward' vehicles that
889 : // do not sideslip; fusing it on a multicopter corrupts the attitude
890 : // and velocity states when dead reckoning (see issue #33451)
891 154797 : if (is_positive(defaultAirSpeed)) {
892 : // this is the preferred method with the autopilot providing a model based airspeed estimate
893 0 : if (imuDataDelayed.time_ms - prevTasStep_ms > 200 ) {
894 0 : tasDataDelayed.tas = defaultAirSpeed * EAS2TAS;
895 0 : tasDataDelayed.tasVariance = MAX(defaultAirSpeedVariance, sq(MAX(frontend->_easNoise, 0.5f)));
896 0 : tasDataToFuse = true;
897 0 : tasDataDelayed.allowFusion = true;
898 0 : tasDataDelayed.time_ms = imuDataDelayed.time_ms;
899 : } else {
900 0 : tasDataToFuse = false;
901 0 : tasDataDelayed.allowFusion = false;
902 : }
903 154797 : } else if (lastAspdEstIsValid && !windStateIsObservable) {
904 : // this uses the last airspeed estimated before dead reckoning started and
905 : // wind states became unobservable
906 6760 : if (lastAspdEstIsValid && imuDataDelayed.time_ms - prevTasStep_ms > 200) {
907 6760 : tasDataDelayed.tas = lastAirspeedEstimate;
908 : // this airspeed estimate has a lot of uncertainty
909 6760 : tasDataDelayed.tasVariance = sq(MAX(MAX(frontend->_easNoise, 0.5f), 0.5f * lastAirspeedEstimate));
910 6760 : tasDataToFuse = true;
911 6760 : tasDataDelayed.allowFusion = true;
912 6760 : tasDataDelayed.time_ms = imuDataDelayed.time_ms;
913 : } else {
914 0 : tasDataToFuse = false;
915 0 : tasDataDelayed.allowFusion = false;
916 : }
917 : }
918 : } else {
919 12993986 : tasDataToFuse = false;
920 12993986 : tasDataDelayed.allowFusion = false;
921 : }
922 18761238 : }
923 :
924 : #if EK3_FEATURE_BEACON_FUSION
925 : /********************************************************
926 : * Range Beacon Measurements *
927 : ********************************************************/
928 :
929 : // check for new range beacon data and push to data buffer if available
930 19965274 : void NavEKF3_core::readRngBcnData()
931 : {
932 : // check that arrays are large enough
933 : static_assert(ARRAY_SIZE(rngBcn.lastTime_ms) >= AP_BEACON_MAX_BEACONS, "lastTimeRngBcn_ms should have at least AP_BEACON_MAX_BEACONS elements");
934 :
935 : // get the location of the beacon data
936 19965274 : const AP_DAL_Beacon *beacon = dal.beacon();
937 :
938 : // exit immediately if no beacon object
939 19965274 : if (beacon == nullptr) {
940 19897480 : return;
941 : }
942 :
943 : // get the number of beacons in use
944 67794 : rngBcn.N = MIN(beacon->count(), ARRAY_SIZE(rngBcn.lastTime_ms));
945 :
946 : // search through all the beacons for new data and if we find it stop searching and push the data into the observation buffer
947 : bool newDataPushed = false;
948 : uint8_t numRngBcnsChecked = 0;
949 : // start the search one index up from where we left it last time
950 67794 : uint8_t index = rngBcn.lastChecked;
951 176397 : while (!newDataPushed && (numRngBcnsChecked < rngBcn.N)) {
952 : // track the number of beacons checked
953 108603 : numRngBcnsChecked++;
954 :
955 : // move to next beacon, wrap index if necessary
956 108603 : index++;
957 108603 : if (index >= rngBcn.N) {
958 : index = 0;
959 : }
960 :
961 : // check that the beacon is healthy and has new data
962 108603 : if (beacon->beacon_healthy(index) && beacon->beacon_last_update_ms(index) != rngBcn.lastTime_ms[index]) {
963 54191 : rng_bcn_elements rngBcnDataNew = {};
964 :
965 : // set the timestamp, correcting for measurement delay and average intersampling delay due to the filter update rate
966 54191 : rngBcn.lastTime_ms[index] = beacon->beacon_last_update_ms(index);
967 54191 : rngBcnDataNew.time_ms = rngBcn.lastTime_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2;
968 :
969 : // set the range noise
970 : // TODO the range library should provide the noise/accuracy estimate for each beacon
971 54191 : rngBcnDataNew.rngErr = frontend->_rngBcnNoise.get();
972 :
973 : // set the range measurement
974 54191 : rngBcnDataNew.rng = beacon->beacon_distance(index);
975 :
976 : // set the beacon position
977 54191 : rngBcnDataNew.beacon_posNED = beacon->beacon_position(index).toftype();
978 :
979 : // identify the beacon identifier
980 54191 : rngBcnDataNew.beacon_ID = index;
981 :
982 : // indicate we have new data to push to the buffer
983 : newDataPushed = true;
984 :
985 : // update the last checked index
986 54191 : rngBcn.lastChecked = index;
987 :
988 : // Save data into the buffer to be fused when the fusion time horizon catches up with it
989 54191 : rngBcn.storedRange.push(rngBcnDataNew);
990 : }
991 : }
992 :
993 : // Check if the beacon system has returned a 3D fix
994 67794 : Vector3f bp;
995 : float bperr;
996 67794 : if (beacon->get_vehicle_position_ned(bp, bperr)) {
997 67401 : rngBcn.last3DmeasTime_ms = imuSampleTime_ms;
998 : }
999 67794 : rngBcn.vehiclePosNED = bp.toftype();
1000 67794 : rngBcn.vehiclePosErr = bperr;
1001 :
1002 : // Check if the range beacon data can be used to align the vehicle position
1003 67794 : if ((imuSampleTime_ms - rngBcn.last3DmeasTime_ms < 250) && (rngBcn.vehiclePosErr < 1.0f) && rngBcn.alignmentCompleted) {
1004 : // check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
1005 64323 : const ftype posDiffSq = sq(rngBcn.receiverPos.x - rngBcn.vehiclePosNED.x) + sq(rngBcn.receiverPos.y - rngBcn.vehiclePosNED.y);
1006 64323 : const ftype posDiffVar = sq(rngBcn.vehiclePosErr) + rngBcn.receiverPosCov[0][0] + rngBcn.receiverPosCov[1][1];
1007 64323 : if (posDiffSq < 9.0f * posDiffVar) {
1008 40404 : rngBcn.goodToAlign = true;
1009 : // Set the EKF origin and magnetic field declination if not previously set
1010 40404 : if (!validOrigin && (PV_AidingMode != AID_ABSOLUTE)) {
1011 : // get origin from beacon system
1012 6 : Location origin_loc;
1013 6 : if (beacon->get_origin(origin_loc)) {
1014 6 : setOriginLLH(origin_loc);
1015 :
1016 : // set the NE earth magnetic field states using the published declination
1017 : // and set the corresponding variances and covariances
1018 6 : alignMagStateDeclination();
1019 :
1020 : // Set the uncertainty of the origin height
1021 6 : ekfOriginHgtVar = sq(rngBcn.vehiclePosErr);
1022 : }
1023 : }
1024 : } else {
1025 23919 : rngBcn.goodToAlign = false;
1026 : }
1027 : } else {
1028 3471 : rngBcn.goodToAlign = false;
1029 : }
1030 :
1031 : // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
1032 67794 : rngBcn.dataToFuse = rngBcn.storedRange.recall(rngBcn.dataDelayed, imuDataDelayed.time_ms);
1033 :
1034 : // Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin
1035 67794 : if (rngBcn.dataToFuse) {
1036 48202 : rngBcn.dataDelayed.beacon_posNED.x += rngBcn.posOffsetNED.x;
1037 48202 : rngBcn.dataDelayed.beacon_posNED.y += rngBcn.posOffsetNED.y;
1038 : }
1039 :
1040 : }
1041 : #endif // EK3_FEATURE_BEACON_FUSION
1042 :
1043 : /********************************************************
1044 : * Independant yaw sensor measurements *
1045 : ********************************************************/
1046 :
1047 10752 : void NavEKF3_core::writeEulerYawAngle(float yawAngle, float yawAngleErr, uint32_t timeStamp_ms, uint8_t type, const Vector3f &antOffset)
1048 : {
1049 : // limit update rate to maximum allowed by sensor buffers and fusion process
1050 : // don't try to write to buffer until the filter has been initialised
1051 10752 : if (((timeStamp_ms - yawMeasTime_ms) < frontend->sensorIntervalMin_ms) || !statesInitialised) {
1052 : return;
1053 : }
1054 :
1055 5098 : yawAngDataNew.yawAng = yawAngle;
1056 5098 : yawAngDataNew.yawAngErr = yawAngleErr;
1057 5098 : if (type == 2) {
1058 5098 : yawAngDataNew.order = rotationOrder::TAIT_BRYAN_321;
1059 0 : } else if (type == 1) {
1060 0 : yawAngDataNew.order = rotationOrder::TAIT_BRYAN_312;
1061 : } else {
1062 : return;
1063 : }
1064 : // Prevent time delay exceeding age of oldest IMU data in the buffer. Without
1065 : // this a configured delay larger than the fusion time horizon leaves every
1066 : // measurement outside the recall window and the yaw source silently stops
1067 : // fusing. The raw timestamp is still used for new-measurement detection and
1068 : // rate limiting via yawMeasTime_ms
1069 5098 : yawAngDataNew.time_ms = MAX(timeStamp_ms, imuDataDelayed.time_ms);
1070 : #if EK3_FEATURE_MOVING_BASELINE
1071 5098 : yawAngDataNew.antOffset = antOffset.toftype();
1072 : #endif
1073 :
1074 5098 : storedYawAng.push(yawAngDataNew);
1075 :
1076 5098 : yawMeasTime_ms = timeStamp_ms;
1077 : }
1078 :
1079 : // Writes the default equivalent airspeed and 1-sigma uncertainty in m/s to be used in forward flight if a measured airspeed is required and not available.
1080 0 : void NavEKF3_core::writeDefaultAirSpeed(float airspeed, float uncertainty)
1081 : {
1082 0 : defaultAirSpeed = airspeed;
1083 0 : defaultAirSpeedVariance = sq(uncertainty);
1084 0 : }
1085 :
1086 : /********************************************************
1087 : * External Navigation Measurements *
1088 : ********************************************************/
1089 :
1090 19896 : void NavEKF3_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
1091 : {
1092 : #if EK3_FEATURE_EXTERNAL_NAV
1093 : // protect against NaN
1094 19896 : if (pos.is_nan() || isnan(posErr)) {
1095 248 : return;
1096 : }
1097 :
1098 : // limit update rate to maximum allowed by sensor buffers and fusion process
1099 : // don't try to write to buffer until the filter has been initialised
1100 19896 : if (((timeStamp_ms - extNavMeasTime_ms) < frontend->extNavIntervalMin_ms) || !statesInitialised) {
1101 : return;
1102 : } else {
1103 19648 : extNavMeasTime_ms = timeStamp_ms;
1104 : }
1105 :
1106 19648 : ext_nav_elements extNavDataNew {};
1107 :
1108 19648 : if (resetTime_ms != extNavLastPosResetTime_ms) {
1109 0 : extNavDataNew.posReset = true;
1110 0 : extNavLastPosResetTime_ms = resetTime_ms;
1111 : } else {
1112 19648 : extNavDataNew.posReset = false;
1113 : }
1114 :
1115 19648 : extNavDataNew.pos = pos.toftype();
1116 19648 : extNavDataNew.posErr = posErr;
1117 :
1118 : // calculate timestamp
1119 19648 : timeStamp_ms = timeStamp_ms - delay_ms;
1120 : // Correct for the average intersampling delay due to the filter update rate
1121 19648 : timeStamp_ms -= localFilterTimeStep_ms/2;
1122 : // Prevent time delay exceeding age of oldest IMU data in the buffer
1123 19648 : timeStamp_ms = MAX(timeStamp_ms, imuDataDelayed.time_ms);
1124 19648 : extNavDataNew.time_ms = timeStamp_ms;
1125 :
1126 : // store position data to buffer
1127 19648 : storedExtNav.push(extNavDataNew);
1128 :
1129 : // protect against attitude or angle being NaN
1130 19648 : if (!quat.is_nan() && !isnan(angErr)) {
1131 : // extract yaw from the attitude
1132 : ftype roll_rad, pitch_rad, yaw_rad;
1133 19648 : quat.to_euler(roll_rad, pitch_rad, yaw_rad);
1134 19648 : yaw_elements extNavYawAngDataNew;
1135 19648 : extNavYawAngDataNew.yawAng = yaw_rad;
1136 19648 : extNavYawAngDataNew.yawAngErr = MAX(angErr, radians(5.0f)); // ensure yaw accuracy is no better than 5 degrees (some callers may send zero)
1137 19648 : extNavYawAngDataNew.order = rotationOrder::TAIT_BRYAN_321; // Euler rotation order is 321 (ZYX)
1138 19648 : extNavYawAngDataNew.time_ms = timeStamp_ms;
1139 19648 : storedExtNavYawAng.push(extNavYawAngDataNew);
1140 : }
1141 : #endif // EK3_FEATURE_EXTERNAL_NAV
1142 : }
1143 :
1144 8468 : void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
1145 : {
1146 : #if EK3_FEATURE_EXTERNAL_NAV
1147 : // sanity check for NaNs
1148 8468 : if (vel.is_nan() || isnan(err)) {
1149 0 : return;
1150 : }
1151 :
1152 8468 : if ((timeStamp_ms - extNavVelMeasTime_ms) < frontend->extNavIntervalMin_ms) {
1153 : return;
1154 : }
1155 :
1156 8468 : extNavVelMeasTime_ms = timeStamp_ms;
1157 8468 : useExtNavVel = true;
1158 : // calculate timestamp
1159 8468 : timeStamp_ms = timeStamp_ms - delay_ms;
1160 : // Correct for the average intersampling delay due to the filter updaterate
1161 8468 : timeStamp_ms -= localFilterTimeStep_ms/2;
1162 : // Prevent time delay exceeding age of oldest IMU data in the buffer
1163 8468 : timeStamp_ms = MAX(timeStamp_ms,imuDataDelayed.time_ms);
1164 :
1165 8468 : ext_nav_vel_elements extNavVelNew;
1166 8468 : extNavVelNew.time_ms = timeStamp_ms;
1167 8468 : extNavVelNew.vel = vel.toftype();
1168 8468 : extNavVelNew.err = err;
1169 8468 : extNavVelNew.corrected = false;
1170 :
1171 8468 : storedExtNavVel.push(extNavVelNew);
1172 : #endif // EK3_FEATURE_EXTERNAL_NAV
1173 : }
1174 :
1175 : /*
1176 : * Write terrain altitude (derived from SRTM) in meters above the origin
1177 : * only used by optical flow when out of rangefinder range
1178 : */
1179 : #if EK3_FEATURE_OPTFLOW_SRTM
1180 0 : void NavEKF3_core::writeTerrainData(float alt_m)
1181 : {
1182 0 : terrain_srtm_alt = alt_m;
1183 0 : terrain_srtm_alt_ms = imuSampleTime_ms;
1184 0 : }
1185 : #endif
1186 :
1187 : /*
1188 : update the GPS selection
1189 : */
1190 79784903 : void NavEKF3_core::update_gps_selection(void)
1191 : {
1192 79784903 : const auto &gps = dal.gps();
1193 :
1194 : // in normal operation use the primary GPS
1195 79784903 : selected_gps = gps.primary_sensor();
1196 79784903 : preferred_gps = selected_gps;
1197 :
1198 79784903 : if (frontend->_affinity & EKF_AFFINITY_GPS) {
1199 229862 : if (core_index < gps.num_sensors() ) {
1200 : // always prefer our core_index, unless we don't have that
1201 : // many GPS sensors available
1202 213063 : preferred_gps = core_index;
1203 : }
1204 229862 : if (gps.status(preferred_gps) >= AP_GPS_FixType::FIX_3D) {
1205 : // select our preferred_gps if it has a 3D fix, otherwise
1206 : // use the primary GPS
1207 213608 : selected_gps = preferred_gps;
1208 : }
1209 : }
1210 79784903 : }
1211 :
1212 : /*
1213 : update the mag selection
1214 : */
1215 79784903 : void NavEKF3_core::update_mag_selection(void)
1216 : {
1217 79784903 : const auto &compass = dal.compass();
1218 :
1219 79784903 : if (frontend->_affinity & EKF_AFFINITY_MAG) {
1220 1500316 : if (core_index < compass.get_count() &&
1221 750158 : compass.healthy(core_index) &&
1222 750158 : compass.use_for_yaw(core_index)) {
1223 : // use core_index compass if it is healthy
1224 750158 : magSelectIndex = core_index;
1225 : }
1226 : } else {
1227 : // if we are not armed, use the primary compass (if it is healthy)
1228 79034745 : const uint8_t primary_compass = compass.get_first_usable();
1229 79034745 : if (!dal.get_armed() && (magSelectIndex != primary_compass)) {
1230 15 : tryChangeCompass(primary_compass);
1231 : }
1232 : }
1233 79784903 : }
1234 :
1235 : /*
1236 : update the baro selection
1237 : */
1238 79784903 : void NavEKF3_core::update_baro_selection(void)
1239 : {
1240 79784903 : auto &baro = dal.baro();
1241 :
1242 : // in normal operation use the primary baro
1243 79784903 : selected_baro = baro.get_primary();
1244 :
1245 79784903 : if (frontend->_affinity & EKF_AFFINITY_BARO) {
1246 27764 : if (core_index < baro.num_instances() &&
1247 27764 : baro.healthy(core_index)) {
1248 : // use core_index baro if it is healthy
1249 27764 : selected_baro = core_index;
1250 : }
1251 : }
1252 79784903 : }
1253 :
1254 : /*
1255 : update the airspeed selection
1256 : */
1257 79784903 : void NavEKF3_core::update_airspeed_selection(void)
1258 : {
1259 79784903 : const auto *arsp = dal.airspeed();
1260 79784903 : if (arsp == nullptr) {
1261 : return;
1262 : }
1263 :
1264 : // in normal operation use the primary airspeed sensor
1265 16177488 : selected_airspeed = arsp->get_primary();
1266 :
1267 16177488 : if (frontend->_affinity & EKF_AFFINITY_ARSP) {
1268 55470 : if (core_index < arsp->get_num_sensors() &&
1269 27764 : arsp->healthy(core_index) &&
1270 27672 : arsp->use(core_index)) {
1271 : // use core_index airspeed if it is healthy
1272 27672 : selected_airspeed = core_index;
1273 : }
1274 : }
1275 : }
1276 :
1277 : /*
1278 : update sensor selections
1279 : */
1280 79784903 : void NavEKF3_core::update_sensor_selection(void)
1281 : {
1282 79784903 : update_gps_selection();
1283 79784903 : update_mag_selection();
1284 79784903 : update_baro_selection();
1285 79784903 : update_airspeed_selection();
1286 79784903 : }
1287 :
1288 : /*
1289 : update timing statistics structure
1290 : */
1291 20078569 : void NavEKF3_core::updateTimingStatistics(void)
1292 : {
1293 20078569 : if (timing.count == 0) {
1294 59103 : timing.dtIMUavg_max = dtIMUavg;
1295 59103 : timing.dtIMUavg_min = dtIMUavg;
1296 59103 : timing.dtEKFavg_max = dtEkfAvg;
1297 59103 : timing.dtEKFavg_min = dtEkfAvg;
1298 59103 : timing.delAngDT_max = imuDataDelayed.delAngDT;
1299 59103 : timing.delAngDT_min = imuDataDelayed.delAngDT;
1300 59103 : timing.delVelDT_max = imuDataDelayed.delVelDT;
1301 59103 : timing.delVelDT_min = imuDataDelayed.delVelDT;
1302 : } else {
1303 20019466 : timing.dtIMUavg_max = MAX(timing.dtIMUavg_max, dtIMUavg);
1304 20019466 : timing.dtIMUavg_min = MIN(timing.dtIMUavg_min, dtIMUavg);
1305 20019466 : timing.dtEKFavg_max = MAX(timing.dtEKFavg_max, dtEkfAvg);
1306 20019466 : timing.dtEKFavg_min = MIN(timing.dtEKFavg_min, dtEkfAvg);
1307 20019466 : timing.delAngDT_max = MAX(timing.delAngDT_max, imuDataDelayed.delAngDT);
1308 20019466 : timing.delAngDT_min = MIN(timing.delAngDT_min, imuDataDelayed.delAngDT);
1309 20019466 : timing.delVelDT_max = MAX(timing.delVelDT_max, imuDataDelayed.delVelDT);
1310 20019466 : timing.delVelDT_min = MIN(timing.delVelDT_min, imuDataDelayed.delVelDT);
1311 : }
1312 20078569 : timing.count++;
1313 20078569 : }
1314 :
1315 : /*
1316 : update estimates of inactive bias states. This keeps inactive IMUs
1317 : as hot-spares so we can switch to them without causing a jump in the
1318 : error
1319 : */
1320 79223583 : void NavEKF3_core::learnInactiveBiases(void)
1321 : {
1322 : #if INS_MAX_INSTANCES == 1
1323 : inactiveBias[0].gyro_bias = stateStruct.gyro_bias;
1324 : inactiveBias[0].accel_bias = stateStruct.accel_bias;
1325 : #else
1326 79223583 : const auto &ins = dal.ins();
1327 :
1328 : // learn gyro biases
1329 316894332 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
1330 237670749 : if (!ins.use_gyro(i)) {
1331 : // can't use this gyro
1332 79032269 : continue;
1333 : }
1334 158638480 : if (gyro_index_active == i) {
1335 : // use current estimates from main filter of gyro bias
1336 79223583 : inactiveBias[i].gyro_bias = stateStruct.gyro_bias;
1337 : } else {
1338 : // get filtered gyro and use the difference between the
1339 : // corrected gyro on the active IMU and the inactive IMU
1340 : // to move the inactive bias towards the right value
1341 79414897 : Vector3F filtered_gyro_active = ins.get_gyro(gyro_index_active).toftype() - (stateStruct.gyro_bias/dtEkfAvg);
1342 79414897 : Vector3F filtered_gyro_inactive = ins.get_gyro(i).toftype() - (inactiveBias[i].gyro_bias/dtEkfAvg);
1343 79414897 : Vector3F error = filtered_gyro_active - filtered_gyro_inactive;
1344 :
1345 : // prevent a single large error from contaminating bias estimate
1346 : const ftype bias_limit = radians(5);
1347 79414897 : error.x = constrain_ftype(error.x, -bias_limit, bias_limit);
1348 79414897 : error.y = constrain_ftype(error.y, -bias_limit, bias_limit);
1349 79414897 : error.z = constrain_ftype(error.z, -bias_limit, bias_limit);
1350 :
1351 : // slowly bring the inactive gyro in line with the active gyro. This corrects a 5 deg/sec
1352 : // gyro bias error in around 1 minute
1353 79414897 : inactiveBias[i].gyro_bias -= error * (1.0e-4f * dtEkfAvg);
1354 : }
1355 : }
1356 :
1357 : // learn accel biases
1358 316894332 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
1359 237670749 : if (!ins.use_accel(i)) {
1360 : // can't use this accel
1361 79032269 : continue;
1362 : }
1363 158638480 : if (accel_index_active == i) {
1364 : // use current estimates from main filter of accel bias
1365 79223583 : inactiveBias[i].accel_bias = stateStruct.accel_bias;
1366 : } else {
1367 : // get filtered accel and use the difference between the
1368 : // corrected accel on the active IMU and the inactive IMU
1369 : // to move the inactive bias towards the right value
1370 79414897 : Vector3F filtered_accel_active = ins.get_accel(accel_index_active).toftype() - (stateStruct.accel_bias/dtEkfAvg);
1371 79414897 : Vector3F filtered_accel_inactive = ins.get_accel(i).toftype() - (inactiveBias[i].accel_bias/dtEkfAvg);
1372 79414897 : Vector3F error = filtered_accel_active - filtered_accel_inactive;
1373 :
1374 : // prevent a single large error from contaminating bias estimate
1375 : const ftype bias_limit = 1.0; // m/s/s
1376 79414897 : error.x = constrain_ftype(error.x, -bias_limit, bias_limit);
1377 79414897 : error.y = constrain_ftype(error.y, -bias_limit, bias_limit);
1378 79414897 : error.z = constrain_ftype(error.z, -bias_limit, bias_limit);
1379 :
1380 : // slowly bring the inactive accel in line with the active
1381 : // accel. This corrects a 0.5 m/s/s accel bias error in
1382 : // around 1 minute
1383 79414897 : inactiveBias[i].accel_bias -= error * (1.0e-4f * dtEkfAvg);
1384 : }
1385 : }
1386 : #endif
1387 79223583 : }
1388 :
1389 : /*
1390 : return declination in radians
1391 : */
1392 2758838 : ftype NavEKF3_core::MagDeclination(void) const
1393 : {
1394 : // if we are using the WMM tables then use the table declination
1395 : // to ensure consistency with the table mag field. Otherwise use
1396 : // the declination from the compass library
1397 2758838 : if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
1398 2510323 : return table_declination;
1399 : }
1400 248515 : if (!use_compass()) {
1401 : return 0;
1402 : }
1403 248494 : return dal.compass().get_declination();
1404 : }
1405 :
1406 : /*
1407 : Update the on ground and not moving check.
1408 : Should be called once per IMU update.
1409 : Used for yaw fusion strategy selection, zero velocity fusion gating,
1410 : and accel bias learning inhibition during ground movement.
1411 : */
1412 79223583 : void NavEKF3_core::updateMovementCheck(void)
1413 : {
1414 79223583 : if (!onGround)
1415 : {
1416 54406915 : onGroundNotMoving = false;
1417 54406915 : return;
1418 : }
1419 :
1420 : const ftype gyro_limit = radians(3.0f);
1421 : const ftype gyro_diff_limit = 0.2f;
1422 : const ftype accel_limit = 1.0f;
1423 : const ftype accel_diff_limit = 5.0f;
1424 : const ftype hysteresis_ratio = 0.7f;
1425 24816668 : const ftype dtEkfAvgInv = 1.0f / dtEkfAvg;
1426 :
1427 : // get latest bias corrected gyro and accelerometer data
1428 24816668 : const auto &ins = dal.ins();
1429 24816668 : Vector3F gyro = ins.get_gyro(gyro_index_active).toftype() - stateStruct.gyro_bias * dtEkfAvgInv;
1430 24816668 : Vector3F accel = ins.get_accel(accel_index_active).toftype() - stateStruct.accel_bias * dtEkfAvgInv;
1431 :
1432 24816668 : if (!prevOnGround) {
1433 0 : gyro_prev = gyro;
1434 0 : accel_prev = accel;
1435 0 : onGroundNotMoving = false;
1436 0 : gyro_diff = gyro_diff_limit;
1437 0 : accel_diff = accel_diff_limit;
1438 0 : return;
1439 : }
1440 :
1441 : // calculate a gyro rate of change metric
1442 24816668 : Vector3F temp = (gyro - gyro_prev) * dtEkfAvgInv;
1443 24816668 : gyro_prev = gyro;
1444 24816668 : gyro_diff = 0.99f * gyro_diff + 0.01f * temp.length();
1445 :
1446 : // calculate a acceleration rate of change metric
1447 24816668 : temp = (accel - accel_prev) * dtEkfAvgInv;
1448 24816668 : accel_prev = accel;
1449 24816668 : accel_diff = 0.99f * accel_diff + 0.01f * temp.length();
1450 :
1451 24816668 : const ftype gyro_length_ratio = gyro.length() / gyro_limit;
1452 24816668 : const ftype accel_length_ratio = (accel.length() - GRAVITY_MSS) / accel_limit;
1453 24816668 : const ftype gyro_diff_ratio = gyro_diff / gyro_diff_limit;
1454 24816668 : const ftype accel_diff_ratio = accel_diff / accel_diff_limit;
1455 : bool logStatusChange = false;
1456 24816668 : if (onGroundNotMoving) {
1457 23135882 : if (gyro_length_ratio > frontend->_ognmTestScaleFactor ||
1458 23135667 : fabsF(accel_length_ratio) > frontend->_ognmTestScaleFactor ||
1459 46271382 : gyro_diff_ratio > frontend->_ognmTestScaleFactor ||
1460 23135499 : accel_diff_ratio > frontend->_ognmTestScaleFactor)
1461 : {
1462 383 : onGroundNotMoving = false;
1463 : logStatusChange = true;
1464 : }
1465 1680786 : } else if (gyro_length_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
1466 208780 : fabsF(accel_length_ratio) < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
1467 1783457 : gyro_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio &&
1468 61536 : accel_diff_ratio < frontend->_ognmTestScaleFactor * hysteresis_ratio)
1469 : {
1470 5268 : onGroundNotMoving = true;
1471 : logStatusChange = true;
1472 : }
1473 :
1474 24811017 : if (logStatusChange || imuSampleTime_ms - lastMoveCheckLogTime_ms > 200) {
1475 422749 : lastMoveCheckLogTime_ms = imuSampleTime_ms;
1476 : #if HAL_LOGGING_ENABLED
1477 422749 : const struct log_XKFM pkt{
1478 : LOG_PACKET_HEADER_INIT(LOG_XKFM_MSG),
1479 422749 : time_us : dal.micros64(),
1480 422749 : core : core_index,
1481 422749 : ongroundnotmoving : onGroundNotMoving,
1482 422749 : gyro_length_ratio : float(gyro_length_ratio),
1483 422749 : accel_length_ratio : float(accel_length_ratio),
1484 422749 : gyro_diff_ratio : float(gyro_diff_ratio),
1485 422749 : accel_diff_ratio : float(accel_diff_ratio),
1486 422749 : };
1487 422749 : AP::logger().WriteBlock(&pkt, sizeof(pkt));
1488 : #endif
1489 : }
1490 : }
1491 :
1492 20078569 : void NavEKF3_core::SampleDragData(const imu_elements &imu)
1493 : {
1494 : #if EK3_FEATURE_DRAG_FUSION
1495 : // Average and down sample to 5Hz
1496 20078569 : const ftype bcoef_x = frontend->_ballisticCoef_x.get();
1497 20078569 : const ftype bcoef_y = frontend->_ballisticCoef_y.get();
1498 20078569 : const ftype mcoef = frontend->_momentumDragCoef.get();
1499 20078569 : const bool using_bcoef_x = bcoef_x > 1.0f;
1500 20078569 : const bool using_bcoef_y = bcoef_y > 1.0f;
1501 : const bool using_mcoef = mcoef > 0.001f;
1502 20078569 : if (!using_bcoef_x && !using_bcoef_y && !using_mcoef) {
1503 : // nothing to do
1504 19931860 : dragFusionEnabled = false;
1505 19931860 : return;
1506 : }
1507 :
1508 146709 : dragFusionEnabled = true;
1509 :
1510 : // down-sample the drag specific force data by accumulating and calculating the mean when
1511 : // sufficient samples have been collected
1512 :
1513 146709 : dragSampleCount ++;
1514 :
1515 : // note acceleration is accumulated as a delta velocity
1516 146709 : dragDownSampled.accelXY.x += imu.delVel.x;
1517 146709 : dragDownSampled.accelXY.y += imu.delVel.y;
1518 146709 : dragDownSampled.time_ms += imu.time_ms;
1519 146709 : dragSampleTimeDelta += imu.delVelDT;
1520 :
1521 : // calculate and store means from accumulated values
1522 146709 : if (dragSampleTimeDelta > 0.2f - 0.5f * EKF_TARGET_DT) {
1523 : // note conversion from accumulated delta velocity to acceleration
1524 9209 : dragDownSampled.accelXY.x /= dragSampleTimeDelta;
1525 9209 : dragDownSampled.accelXY.y /= dragSampleTimeDelta;
1526 9209 : dragDownSampled.time_ms /= dragSampleCount;
1527 :
1528 : // write to buffer
1529 9209 : storedDrag.push(dragDownSampled);
1530 :
1531 : // reset accumulators
1532 9209 : dragSampleCount = 0;
1533 9209 : dragDownSampled.accelXY.zero();
1534 9209 : dragDownSampled.time_ms = 0;
1535 9209 : dragSampleTimeDelta = 0.0f;
1536 : }
1537 : #endif // EK3_FEATURE_DRAG_FUSION
1538 : }
1539 :
1540 : /*
1541 : get the earth mag field
1542 : */
1543 224185 : void NavEKF3_core::getEarthFieldTable(const Location &loc)
1544 : {
1545 224185 : table_earth_field_ga = AP_Declination::get_earth_field_ga(loc).toftype();
1546 224185 : table_declination = radians(AP_Declination::get_declination(loc.lat*1.0e-7,
1547 224185 : loc.lng*1.0e-7));
1548 224185 : have_table_earth_field = true;
1549 224185 : }
1550 :
1551 : /*
1552 : update earth field, called at 1Hz
1553 : */
1554 280584 : void NavEKF3_core::checkUpdateEarthField(void)
1555 : {
1556 280584 : if (have_table_earth_field && filterStatus.flags.using_gps) {
1557 222459 : Location loc = EKF_origin;
1558 222459 : loc.offset(stateStruct.position.x, stateStruct.position.y);
1559 222459 : getEarthFieldTable(loc);
1560 : }
1561 280584 : }
|
