Line data Source code
1 : #include "AP_NavEKF2_core.h"
2 :
3 : #include "AP_NavEKF2.h"
4 :
5 : #include <GCS_MAVLink/GCS.h>
6 : #include <AP_DAL/AP_DAL.h>
7 : #include <AP_InternalError/AP_InternalError.h>
8 :
9 : #if AP_RANGEFINDER_ENABLED
10 :
11 :
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 3230372 : void NavEKF2_core::readRangeFinder(void)
19 : {
20 : uint8_t midIndex;
21 : uint8_t maxIndex;
22 : uint8_t minIndex;
23 :
24 : // get theoretical correct range when the vehicle is on the ground
25 : // don't allow range to go below 5cm because this can cause problems with optical flow processing
26 3230372 : const auto *_rng = dal.rangefinder();
27 3230372 : if (_rng == nullptr) {
28 : return;
29 : }
30 :
31 55546 : rngOnGnd = MAX(_rng->ground_clearance_orient(ROTATION_PITCH_270), 0.05f);
32 :
33 : // read range finder at 20Hz
34 : // TODO better way of knowing if it has new data
35 55546 : if (_rng && (imuSampleTime_ms - lastRngMeasTime_ms) > 50) {
36 :
37 : // reset the timer used to control the measurement rate
38 11182 : lastRngMeasTime_ms = imuSampleTime_ms;
39 :
40 : // store samples and sample time into a ring buffer if valid
41 : // use data from two range finders if available
42 :
43 33546 : for (uint8_t sensorIndex = 0; sensorIndex < ARRAY_SIZE(rngMeasIndex); sensorIndex++) {
44 22364 : auto *sensor = _rng->get_backend(sensorIndex);
45 22364 : if (sensor == nullptr) {
46 17134 : continue;
47 : }
48 11182 : if ((sensor->orientation() == ROTATION_PITCH_270) && (sensor->status() == AP_DAL_RangeFinder::Status::Good)) {
49 5230 : rngMeasIndex[sensorIndex] ++;
50 5230 : if (rngMeasIndex[sensorIndex] > 2) {
51 1742 : rngMeasIndex[sensorIndex] = 0;
52 : }
53 5230 : storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
54 5230 : storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = sensor->distance();
55 : } else {
56 5952 : continue;
57 : }
58 :
59 : // check for three fresh samples
60 5230 : bool sampleFresh[DOWNWARD_RANGEFINDER_MAX_INSTANCES][3] = {};
61 20920 : for (uint8_t index = 0; index <= 2; index++) {
62 15690 : sampleFresh[sensorIndex][index] = (imuSampleTime_ms - storedRngMeasTime_ms[sensorIndex][index]) < 500;
63 : }
64 :
65 : // find the median value if we have three fresh samples
66 5230 : if (sampleFresh[sensorIndex][0] && sampleFresh[sensorIndex][1] && sampleFresh[sensorIndex][2]) {
67 5206 : if (storedRngMeas[sensorIndex][0] > storedRngMeas[sensorIndex][1]) {
68 : minIndex = 1;
69 : maxIndex = 0;
70 : } else {
71 : minIndex = 0;
72 : maxIndex = 1;
73 : }
74 5206 : if (storedRngMeas[sensorIndex][2] > storedRngMeas[sensorIndex][maxIndex]) {
75 : midIndex = maxIndex;
76 4514 : } else if (storedRngMeas[sensorIndex][2] < storedRngMeas[sensorIndex][minIndex]) {
77 : midIndex = minIndex;
78 : } else {
79 : midIndex = 2;
80 : }
81 :
82 : // don't allow time to go backwards
83 5206 : if (storedRngMeasTime_ms[sensorIndex][midIndex] > rangeDataNew.time_ms) {
84 3092 : rangeDataNew.time_ms = storedRngMeasTime_ms[sensorIndex][midIndex];
85 : }
86 :
87 : // limit the measured range to be no less than the on-ground range
88 5206 : rangeDataNew.rng = MAX(storedRngMeas[sensorIndex][midIndex],rngOnGnd);
89 :
90 : // get position in body frame for the current sensor
91 5206 : rangeDataNew.sensor_idx = sensorIndex;
92 :
93 : // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
94 5206 : storedRange.push(rangeDataNew);
95 :
96 : // indicate we have updated the measurement
97 5206 : rngValidMeaTime_ms = imuSampleTime_ms;
98 :
99 24 : } else if (!takeOffDetected && ((imuSampleTime_ms - rngValidMeaTime_ms) > 200)) {
100 : // before takeoff we assume on-ground range value if there is no data
101 12 : rangeDataNew.time_ms = imuSampleTime_ms;
102 12 : rangeDataNew.rng = rngOnGnd;
103 :
104 : // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
105 12 : storedRange.push(rangeDataNew);
106 :
107 : // indicate we have updated the measurement
108 12 : rngValidMeaTime_ms = imuSampleTime_ms;
109 :
110 : }
111 : }
112 : }
113 : }
114 : #endif
115 :
116 : // write the raw optical flow measurements
117 : // this needs to be called externally.
118 0 : void NavEKF2_core::writeOptFlowMeas(const uint8_t rawFlowQuality, const Vector2f &rawFlowRates, const Vector2f &rawGyroRates, const uint32_t msecFlowMeas, const Vector3f &posOffset, float heightOverride)
119 : {
120 : // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
121 : // The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
122 : // A positive X rate is produced by a positive sensor rotation about the X axis
123 : // A positive Y rate is produced by a positive sensor rotation about the Y axis
124 : // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
125 : // 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
126 0 : flowMeaTime_ms = imuSampleTime_ms;
127 : // calculate bias errors on flow sensor gyro rates, but protect against spikes in data
128 : // reset the accumulated body delta angle and time
129 : // don't do the calculation if not enough time lapsed for a reliable body rate measurement
130 0 : if (delTimeOF > 0.01f) {
131 0 : flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_ftype((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
132 0 : flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_ftype((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
133 0 : delAngBodyOF.zero();
134 0 : delTimeOF = 0.0f;
135 : }
136 : // by definition if this function is called, then flow measurements have been provided so we
137 : // need to run the optical flow takeoff detection
138 0 : detectOptFlowTakeoff();
139 :
140 : // calculate rotation matrices at mid sample time for flow observations
141 0 : stateStruct.quat.rotation_matrix(Tbn_flow);
142 : // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
143 0 : if ((rawFlowQuality > 0) && rawFlowRates.length() < 4.2f && rawGyroRates.length() < 4.2f) {
144 : // correct flow sensor body rates for bias and write
145 0 : ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
146 0 : ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
147 : // the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
148 0 : if (delTimeOF > 0.001f) {
149 : // first preference is to use the rate averaged over the same sampling period as the flow sensor
150 0 : ofDataNew.bodyRadXYZ.z = delAngBodyOF.z / delTimeOF;
151 0 : } else if (imuDataNew.delAngDT > 0.001f){
152 : // second preference is to use most recent IMU data
153 0 : ofDataNew.bodyRadXYZ.z = imuDataNew.delAng.z / imuDataNew.delAngDT;
154 : } else {
155 : // third preference is use zero
156 0 : ofDataNew.bodyRadXYZ.z = 0.0f;
157 : }
158 : // write uncorrected flow rate measurements
159 : // note correction for different axis and sign conventions used by the px4flow sensor
160 0 : ofDataNew.flowRadXY = (-rawFlowRates).toftype(); // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
161 : // write the flow sensor position in body frame
162 0 : ofDataNew.body_offset = posOffset.toftype();
163 : // write the flow sensor height override
164 0 : ofDataNew.heightOverride = heightOverride;
165 : // write flow rate measurements corrected for body rates
166 0 : ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
167 0 : ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.y;
168 : // record time last observation was received so we can detect loss of data elsewhere
169 0 : flowValidMeaTime_ms = imuSampleTime_ms;
170 : // estimate sample time of the measurement
171 0 : ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
172 : // Correct for the average intersampling delay due to the filter updaterate
173 0 : ofDataNew.time_ms -= localFilterTimeStep_ms/2;
174 : // Prevent time delay exceeding age of oldest IMU data in the buffer
175 0 : ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms);
176 : // Save data to buffer
177 0 : storedOF.push(ofDataNew);
178 : // Check for data at the fusion time horizon
179 0 : flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
180 : }
181 0 : }
182 :
183 :
184 : /********************************************************
185 : * MAGNETOMETER *
186 : ********************************************************/
187 :
188 : // try changing to another compass
189 14 : void NavEKF2_core::tryChangeCompass(void)
190 : {
191 14 : const auto &compass = dal.compass();
192 14 : const uint8_t maxCount = compass.get_count();
193 :
194 : // search through the list of magnetometers
195 14 : for (uint8_t i=1; i<maxCount; i++) {
196 14 : uint8_t tempIndex = magSelectIndex + i;
197 : // loop back to the start index if we have exceeded the bounds
198 14 : if (tempIndex >= maxCount) {
199 2 : tempIndex -= maxCount;
200 : }
201 : // if the magnetometer is allowed to be used for yaw and has a different index, we start using it
202 14 : if (compass.healthy(tempIndex) && compass.use_for_yaw(tempIndex) && tempIndex != magSelectIndex) {
203 14 : magSelectIndex = tempIndex;
204 14 : GCS_SEND_TEXT(MAV_SEVERITY_INFO, "EKF2 IMU%u switching to compass %u",(unsigned)imu_index,magSelectIndex);
205 : // reset the timeout flag and timer
206 14 : magTimeout = false;
207 14 : lastHealthyMagTime_ms = imuSampleTime_ms;
208 : // zero the learned magnetometer bias states
209 14 : stateStruct.body_magfield.zero();
210 : // clear the measurement buffer
211 14 : storedMag.reset();
212 : // clear the data waiting flag so that we do not use any data pending from the previous sensor
213 14 : magDataToFuse = false;
214 : // request a reset of the magnetic field states
215 14 : magStateResetRequest = true;
216 : // declare the field unlearned so that the reset request will be obeyed
217 14 : magFieldLearned = false;
218 :
219 : // reset body mag variances on next CovariancePrediction
220 14 : needMagBodyVarReset = true;
221 14 : return;
222 : }
223 : }
224 : }
225 :
226 : // check for new magnetometer data and update store measurements if available
227 3228940 : void NavEKF2_core::readMagData()
228 : {
229 3228940 : const auto &compass = dal.compass();
230 :
231 3228940 : if (!compass.available()) {
232 0 : allMagSensorsFailed = true;
233 0 : return;
234 : }
235 :
236 : // If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
237 : // magnetometer, then declare the magnetometers as failed for this flight
238 3228940 : const uint8_t maxCount = compass.get_count();
239 3228940 : if (allMagSensorsFailed || (magTimeout && assume_zero_sideslip() && magSelectIndex >= maxCount-1 && inFlight)) {
240 2 : allMagSensorsFailed = true;
241 2 : return;
242 : }
243 :
244 3228938 : if (compass.learn_offsets_enabled()) {
245 : // while learning offsets keep all mag states reset
246 0 : InitialiseVariablesMag();
247 0 : wasLearningCompass_ms = imuSampleTime_ms;
248 3228938 : } else if (wasLearningCompass_ms != 0 && imuSampleTime_ms - wasLearningCompass_ms > 1000) {
249 0 : wasLearningCompass_ms = 0;
250 : // force a new yaw alignment 1s after learning completes. The
251 : // delay is to ensure any buffered mag samples are discarded
252 0 : yawAlignComplete = false;
253 0 : InitialiseVariablesMag();
254 : }
255 :
256 : // If the magnetometer has timed out (been rejected for too long), we find another magnetometer to use if available
257 : // 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
258 : // 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
259 : // if the timeout is due to a sensor failure, then declare a timeout regardless of onground status
260 3228938 : if (maxCount > 1) {
261 3228938 : bool fusionTimeout = magTimeout && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000;
262 3228938 : bool sensorTimeout = !compass.healthy(magSelectIndex) && imuSampleTime_ms - lastMagRead_ms > frontend->magFailTimeLimit_ms;
263 3228938 : if (fusionTimeout || sensorTimeout) {
264 14 : tryChangeCompass();
265 : }
266 : }
267 :
268 : // do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
269 : // because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
270 3228938 : if (use_compass() &&
271 3228938 : compass.healthy(magSelectIndex) &&
272 3221382 : compass.last_update_usec(magSelectIndex) - lastMagUpdate_us > 70000) {
273 :
274 : // detect changes to magnetometer offset parameters and reset states
275 610242 : Vector3F nowMagOffsets = compass.get_offsets(magSelectIndex).toftype();
276 610242 : bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
277 : if (changeDetected) {
278 : // zero the learned magnetometer bias states
279 4 : stateStruct.body_magfield.zero();
280 : // clear the measurement buffer
281 4 : storedMag.reset();
282 : // reset body mag variances on next
283 : // CovariancePrediction. This copes with possible errors
284 : // in the new offsets
285 4 : needMagBodyVarReset = true;
286 : }
287 610242 : lastMagOffsets = nowMagOffsets;
288 610242 : lastMagOffsetsValid = true;
289 :
290 : // store time of last measurement update
291 610242 : lastMagUpdate_us = compass.last_update_usec(magSelectIndex);
292 :
293 : // Magnetometer data at the current time horizon
294 610242 : mag_elements magDataNew;
295 :
296 : // estimate of time magnetometer measurement was taken, allowing for delays
297 610242 : magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
298 :
299 : // Correct for the average intersampling delay due to the filter updaterate
300 610242 : magDataNew.time_ms -= localFilterTimeStep_ms/2;
301 :
302 : // read compass data and scale to improve numerical conditioning
303 610242 : magDataNew.mag = compass.get_field(magSelectIndex).toftype() * 0.001f;
304 :
305 : // check for consistent data between magnetometers
306 610242 : consistentMagData = compass.consistent();
307 :
308 : // save magnetometer measurement to buffer to be fused later
309 610242 : storedMag.push(magDataNew);
310 :
311 : // remember time we read compass, to detect compass sensor failure
312 610242 : lastMagRead_ms = imuSampleTime_ms;
313 : }
314 : }
315 :
316 : /********************************************************
317 : * Inertial Measurements *
318 : ********************************************************/
319 :
320 : /*
321 : * Read IMU delta angle and delta velocity measurements and downsample to 100Hz
322 : * for storage in the data buffers used by the EKF. If the IMU data arrives at
323 : * lower rate than 100Hz, then no downsampling or upsampling will be performed.
324 : * Downsampling is done using a method that does not introduce coning or sculling
325 : * errors.
326 : */
327 4231300 : void NavEKF2_core::readIMUData()
328 : {
329 4231300 : const auto &ins = dal.ins();
330 :
331 : // average IMU sampling rate
332 4231300 : dtIMUavg = ins.get_loop_delta_t();
333 :
334 : // use the nominated imu or primary if not available
335 : uint8_t accel_active, gyro_active;
336 :
337 4231300 : if (ins.use_accel(imu_index)) {
338 : accel_active = imu_index;
339 : } else {
340 0 : accel_active = ins.get_first_usable_accel();
341 : }
342 :
343 4231300 : if (ins.use_gyro(imu_index)) {
344 : gyro_active = imu_index;
345 : } else {
346 0 : gyro_active = ins.get_first_usable_gyro();
347 : }
348 :
349 4231300 : if (gyro_active != gyro_index_active) {
350 : // we are switching active gyro at runtime. Copy over the
351 : // biases we have learned from the previously inactive
352 : // gyro. We don't re-init the bias uncertainty as it should
353 : // have the same uncertainty as the previously active gyro
354 0 : stateStruct.gyro_bias = inactiveBias[gyro_active].gyro_bias;
355 0 : gyro_index_active = gyro_active;
356 :
357 : // use the gyro scale factor we have previously used on this
358 : // IMU (if any). We don't reset the variances as we don't want
359 : // errors after switching to be mis-assigned to the gyro scale
360 : // factor
361 0 : stateStruct.gyro_scale = inactiveBias[gyro_active].gyro_scale;
362 : }
363 :
364 4231300 : if (accel_active != accel_index_active) {
365 : // switch to the learned accel bias for this IMU
366 0 : stateStruct.accel_zbias = inactiveBias[accel_active].accel_zbias;
367 0 : accel_index_active = accel_active;
368 : }
369 :
370 : // update the inactive bias states
371 4231300 : learnInactiveBiases();
372 :
373 4231300 : readDeltaVelocity(accel_index_active, imuDataNew.delVel, imuDataNew.delVelDT);
374 4231300 : accelPosOffset = ins.get_imu_pos_offset(accel_index_active).toftype();
375 4231300 : imuDataNew.accel_index = accel_index_active;
376 :
377 : // Get delta angle data from primary gyro or primary if not available
378 4231300 : readDeltaAngle(gyro_index_active, imuDataNew.delAng, imuDataNew.delAngDT);
379 4231300 : imuDataNew.gyro_index = gyro_index_active;
380 :
381 : // Get current time stamp
382 4231300 : imuDataNew.time_ms = imuSampleTime_ms;
383 :
384 : // use the most recent IMU index for the downsampled IMU
385 : // data. This isn't strictly correct if we switch IMUs between
386 : // samples
387 4231300 : imuDataDownSampledNew.gyro_index = imuDataNew.gyro_index;
388 4231300 : imuDataDownSampledNew.accel_index = imuDataNew.accel_index;
389 :
390 : // Accumulate the measurement time interval for the delta velocity and angle data
391 4231300 : imuDataDownSampledNew.delAngDT += imuDataNew.delAngDT;
392 4231300 : imuDataDownSampledNew.delVelDT += imuDataNew.delVelDT;
393 :
394 : // Rotate quaternon atitude from previous to new and normalise.
395 : // Accumulation using quaternions prevents introduction of coning errors due to downsampling
396 4231300 : imuQuatDownSampleNew.rotate(imuDataNew.delAng);
397 4231300 : imuQuatDownSampleNew.normalize();
398 :
399 : // Rotate the latest delta velocity into body frame at the start of accumulation
400 4231300 : Matrix3F deltaRotMat;
401 4231300 : imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
402 :
403 : // Apply the delta velocity to the delta velocity accumulator
404 4231300 : imuDataDownSampledNew.delVel += deltaRotMat*imuDataNew.delVel;
405 :
406 : // Keep track of the number of IMU frames since the last state prediction
407 4231300 : framesSincePredict++;
408 :
409 : /*
410 : * If the target EKF time step has been accumulated, and the frontend has allowed start of a new predict cycle,
411 : * then store the accumulated IMU data to be used by the state prediction, ignoring the frontend permission if more
412 : * than twice the target time has lapsed. Adjust the target EKF step time threshold to allow for timing jitter in the
413 : * IMU data.
414 : */
415 4231300 : if ((dtIMUavg*(float)framesSincePredict >= (EKF_TARGET_DT-(dtIMUavg*0.5)) &&
416 4231300 : startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) {
417 :
418 : // convert the accumulated quaternion to an equivalent delta angle
419 3256540 : imuQuatDownSampleNew.to_axis_angle(imuDataDownSampledNew.delAng);
420 :
421 : // Time stamp the data
422 3256540 : imuDataDownSampledNew.time_ms = imuSampleTime_ms;
423 :
424 : // Write data to the FIFO IMU buffer
425 3256540 : storedIMU.push_youngest_element(imuDataDownSampledNew);
426 :
427 : // calculate the achieved average time step rate for the EKF
428 3256540 : ftype dtNow = constrain_ftype(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.0f,10.0f*EKF_TARGET_DT);
429 3256540 : dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow;
430 :
431 : // zero the accumulated IMU data and quaternion
432 3256540 : imuDataDownSampledNew.delAng.zero();
433 3256540 : imuDataDownSampledNew.delVel.zero();
434 3256540 : imuDataDownSampledNew.delAngDT = 0.0f;
435 3256540 : imuDataDownSampledNew.delVelDT = 0.0f;
436 3256540 : imuDataDownSampledNew.gyro_index = gyro_index_active;
437 3256540 : imuDataDownSampledNew.accel_index = accel_index_active;
438 3256540 : imuQuatDownSampleNew[0] = 1.0f;
439 3256540 : imuQuatDownSampleNew[3] = imuQuatDownSampleNew[2] = imuQuatDownSampleNew[1] = 0.0f;
440 :
441 : // reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle
442 3256540 : framesSincePredict = 0;
443 :
444 : // set the flag to let the filter know it has new IMU data and needs to run
445 3256540 : runUpdates = true;
446 :
447 : // extract the oldest available data from the FIFO buffer
448 3256540 : imuDataDelayed = storedIMU.get_oldest_element();
449 :
450 : // protect against delta time going to zero
451 : // TODO - check if calculations can tolerate 0
452 3256540 : ftype minDT = 0.1f*dtEkfAvg;
453 3256540 : imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
454 3256540 : imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);
455 :
456 3256540 : updateTimingStatistics();
457 :
458 : // correct the extracted IMU data for sensor errors
459 3256540 : delAngCorrected = imuDataDelayed.delAng;
460 3256540 : delVelCorrected = imuDataDelayed.delVel;
461 3256540 : correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT, imuDataDelayed.gyro_index);
462 3256540 : correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT, imuDataDelayed.accel_index);
463 :
464 3256540 : } else {
465 : // we don't have new IMU data in the buffer so don't run filter updates on this time step
466 974760 : runUpdates = false;
467 : }
468 4231300 : }
469 :
470 : // read the delta velocity and corresponding time interval from the IMU
471 : // return false if data is not available
472 4231300 : bool NavEKF2_core::readDeltaVelocity(uint8_t ins_index, Vector3F &dVel, ftype &dVel_dt) {
473 4231300 : const auto &ins = dal.ins();
474 :
475 4231300 : if (ins_index < ins.get_accel_count()) {
476 4231300 : Vector3f dVelF;
477 : float dVel_dtF;
478 4231300 : ins.get_delta_velocity(ins_index, dVelF, dVel_dtF);
479 4231300 : dVel = dVelF.toftype();
480 4231300 : dVel_dt = dVel_dtF;
481 4231300 : dVel_dt = MAX(dVel_dt,1.0e-4f);
482 4231300 : dVel_dt = MIN(dVel_dt,1.0e-1f);
483 : return true;
484 : }
485 : return false;
486 : }
487 :
488 : /********************************************************
489 : * Global Position Measurement *
490 : ********************************************************/
491 :
492 : // check for new valid GPS data and update stored measurement if available
493 3247888 : void NavEKF2_core::readGpsData()
494 : {
495 3247888 : if (frontend->_fusionModeGPS == 3) {
496 : // don't read GPS data if GPS usage disabled
497 : return;
498 : }
499 :
500 : // check for new GPS data
501 : // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
502 3247888 : const auto &gps = dal.gps();
503 3247888 : if (gps.last_message_time_ms(gps.primary_sensor()) - lastTimeGpsReceived_ms > 70) {
504 336830 : if (gps.status() >= AP_GPS_FixType::FIX_3D) {
505 : // report GPS fix status
506 312142 : gpsCheckStatus.bad_fix = false;
507 :
508 : // store fix time from previous read
509 : const uint32_t secondLastGpsTime_ms = lastTimeGpsReceived_ms;
510 :
511 : // get current fix time
512 312142 : lastTimeGpsReceived_ms = gps.last_message_time_ms(gps.primary_sensor());
513 :
514 :
515 : // estimate when the GPS fix was valid, allowing for GPS processing and other delays
516 : // ideally we should be using a timing signal from the GPS receiver to set this time
517 312142 : float gps_delay = 0.0;
518 312142 : gps.get_lag(gps_delay); // ignore the return value
519 312142 : gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(1e3f * gps_delay);
520 :
521 : // Correct for the average intersampling delay due to the filter updaterate
522 312142 : gpsDataNew.time_ms -= localFilterTimeStep_ms/2;
523 :
524 : // Prevent time delay exceeding age of oldest IMU data in the buffer
525 312142 : gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms);
526 :
527 : // Get which GPS we are using for position information
528 312142 : gpsDataNew.sensor_idx = gps.primary_sensor();
529 :
530 : // read the NED velocity from the GPS
531 312142 : gpsDataNew.vel = gps.velocity().toftype();
532 :
533 : // Use the speed and position accuracy from the GPS if available, otherwise set it to zero.
534 : // Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data
535 312142 : ftype alpha = constrain_ftype(0.0002f * (lastTimeGpsReceived_ms - secondLastGpsTime_ms),0.0f,1.0f);
536 312142 : gpsSpdAccuracy *= (1.0f - alpha);
537 : float gpsSpdAccRaw;
538 312142 : if (!gps.speed_accuracy(gpsSpdAccRaw)) {
539 0 : gpsSpdAccuracy = 0.0f;
540 : } else {
541 312142 : gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
542 312142 : gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f);
543 312142 : gpsSpdAccuracy = MAX(gpsSpdAccuracy,frontend->_gpsHorizVelNoise);
544 : }
545 312142 : gpsPosAccuracy *= (1.0f - alpha);
546 : float gpsPosAccRaw;
547 312142 : if (!gps.horizontal_accuracy(gpsPosAccRaw)) {
548 0 : gpsPosAccuracy = 0.0f;
549 : } else {
550 312142 : gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw);
551 312142 : gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f);
552 312142 : gpsPosAccuracy = MAX(gpsPosAccuracy, frontend->_gpsHorizPosNoise);
553 : }
554 312142 : gpsHgtAccuracy *= (1.0f - alpha);
555 : float gpsHgtAccRaw;
556 312142 : if (!gps.vertical_accuracy(gpsHgtAccRaw)) {
557 0 : gpsHgtAccuracy = 0.0f;
558 : } else {
559 312142 : gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw);
560 312142 : gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f);
561 312142 : gpsHgtAccuracy = MAX(gpsHgtAccuracy, 1.5f * frontend->_gpsHorizPosNoise);
562 : }
563 :
564 : // check if we have enough GPS satellites and increase the gps noise scaler if we don't
565 312142 : if (gps.num_sats() >= 6 && (PV_AidingMode == AID_ABSOLUTE)) {
566 254938 : gpsNoiseScaler = 1.0f;
567 57204 : } else if (gps.num_sats() == 5 && (PV_AidingMode == AID_ABSOLUTE)) {
568 0 : gpsNoiseScaler = 1.4f;
569 : } else { // <= 4 satellites or in constant position mode
570 57204 : gpsNoiseScaler = 2.0f;
571 : }
572 :
573 : // Check if GPS can output vertical velocity, if it is allowed to be used, and set GPS fusion mode accordingly
574 312142 : if (gps.have_vertical_velocity() && frontend->_fusionModeGPS == 0) {
575 312142 : useGpsVertVel = true;
576 : } else {
577 0 : useGpsVertVel = false;
578 : }
579 :
580 : // Monitor quality of the GPS velocity data both before and after alignment. This updates
581 : // GpsGoodToAlign class variable
582 312142 : calcGpsGoodToAlign();
583 :
584 : // Post-alignment checks
585 312142 : calcGpsGoodForFlight();
586 :
587 : // see if we can get origin from frontend
588 312142 : if (!validOrigin && frontend->common_origin_valid) {
589 :
590 180 : if (!setOrigin(frontend->common_EKF_origin)) {
591 : // set an error as an attempt was made to set the origin more than once
592 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
593 0 : return;
594 : }
595 :
596 : }
597 :
598 : // Read the GPS location in WGS-84 lat,long,height coordinates
599 312142 : const Location &gpsloc = gps.location();
600 :
601 : // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
602 312142 : if (gpsGoodToAlign && !validOrigin) {
603 180 : Location gpsloc_fieldelevation = gpsloc;
604 : // if flying, correct for height change from takeoff so that the origin is at field elevation
605 180 : if (inFlight) {
606 0 : gpsloc_fieldelevation.alt += (int32_t)(100.0f * stateStruct.position.z);
607 : }
608 :
609 180 : if (!setOrigin(gpsloc_fieldelevation)) {
610 : // set an error as an attempt was made to set the origin more than once
611 0 : INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
612 0 : return;
613 : }
614 :
615 : // set the NE earth magnetic field states using the published declination
616 : // and set the corresponding variances and covariances
617 180 : alignMagStateDeclination();
618 :
619 : // Set the height of the NED origin
620 180 : ekfGpsRefHgt = (double)0.01 * (double)gpsloc.alt + (double)outputDataNew.position.z;
621 :
622 : // Set the uncertainty of the GPS origin height
623 180 : ekfOriginHgtVar = sq(gpsHgtAccuracy);
624 :
625 : }
626 :
627 312142 : if (gpsGoodToAlign && !have_table_earth_field) {
628 394 : const auto &compass = dal.compass();
629 394 : if (compass.have_scale_factor(magSelectIndex) &&
630 394 : compass.auto_declination_enabled()) {
631 394 : table_earth_field_ga = AP_Declination::get_earth_field_ga(gpsloc).toftype();
632 394 : table_declination = radians(AP_Declination::get_declination(gpsloc.lat*1.0e-7,
633 394 : gpsloc.lng*1.0e-7));
634 394 : have_table_earth_field = true;
635 394 : if (frontend->_mag_ef_limit > 0) {
636 : // initialise earth field from tables
637 394 : stateStruct.earth_magfield = table_earth_field_ga;
638 : }
639 : }
640 : }
641 :
642 : // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
643 312142 : if (validOrigin) {
644 287738 : gpsDataNew.pos = EKF_origin.get_distance_NE_ftype(gpsloc);
645 287738 : if ((frontend->_originHgtMode & (1<<2)) == 0) {
646 287738 : gpsDataNew.hgt = (float)((double)0.01 * (double)gpsloc.alt - ekfGpsRefHgt);
647 : } else {
648 0 : gpsDataNew.hgt = 0.01 * (gpsloc.alt - EKF_origin.alt);
649 : }
650 287738 : storedGPS.push(gpsDataNew);
651 : // declare GPS available for use
652 287738 : gpsNotAvailable = false;
653 : }
654 :
655 : } else {
656 : // report GPS fix status
657 24688 : gpsCheckStatus.bad_fix = true;
658 24688 : dal.snprintf(prearm_fail_string, sizeof(prearm_fail_string), "Waiting for 3D fix");
659 : }
660 : }
661 : }
662 :
663 : // read the delta angle and corresponding time interval from the IMU
664 : // return false if data is not available
665 4231300 : bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3F &dAng, ftype &dAng_dt) {
666 4231300 : const auto &ins = dal.ins();
667 :
668 4231300 : if (ins_index < ins.get_gyro_count()) {
669 4231300 : Vector3f dAngF;
670 : float dAng_dtF;
671 4231300 : ins.get_delta_angle(ins_index, dAngF, dAng_dtF);
672 4231300 : dAng = dAngF.toftype();
673 4231300 : dAng_dt = dAng_dtF;
674 4231300 : dAng_dt = MAX(dAng_dt,1.0e-4f);
675 4231300 : dAng_dt = MIN(dAng_dt,1.0e-1f);
676 : return true;
677 : }
678 : return false;
679 : }
680 :
681 :
682 : /********************************************************
683 : * Height Measurements *
684 : ********************************************************/
685 :
686 : // check for new pressure altitude measurement data and update stored measurement if available
687 3247888 : void NavEKF2_core::readBaroData()
688 : {
689 : // check to see if baro measurement has changed so we know if a new measurement has arrived
690 : // do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
691 3247888 : const auto &baro = dal.baro();
692 3247888 : if (baro.get_last_update() - lastBaroReceived_ms > 70) {
693 :
694 616346 : baroDataNew.hgt = baro.get_altitude();
695 :
696 : // If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
697 : // This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
698 616346 : if (dal.get_takeoff_expected() && !assume_zero_sideslip()) {
699 1226 : baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
700 : }
701 :
702 : // time stamp used to check for new measurement
703 616346 : lastBaroReceived_ms = baro.get_last_update();
704 :
705 : // estimate of time height measurement was taken, allowing for delays
706 616346 : baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms;
707 :
708 : // Correct for the average intersampling delay due to the filter updaterate
709 616346 : baroDataNew.time_ms -= localFilterTimeStep_ms/2;
710 :
711 : // Prevent time delay exceeding age of oldest IMU data in the buffer
712 616346 : baroDataNew.time_ms = MAX(baroDataNew.time_ms,imuDataDelayed.time_ms);
713 :
714 : // save baro measurement to buffer to be fused later
715 616346 : storedBaro.push(baroDataNew);
716 : }
717 3247888 : }
718 :
719 : // calculate filtered offset between baro height measurement and EKF height estimate
720 : // offset should be subtracted from baro measurement to match filter estimate
721 : // offset is used to enable reversion to baro from alternate height data source
722 0 : void NavEKF2_core::calcFiltBaroOffset()
723 : {
724 : // Apply a first order LPF with spike protection
725 0 : baroHgtOffset += 0.1f * constrain_ftype(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
726 0 : }
727 :
728 : // correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
729 0 : void NavEKF2_core::correctEkfOriginHeight()
730 : {
731 : // Estimate the WGS-84 height of the EKF's origin using a Bayes filter
732 :
733 : // calculate the variance of our a-priori estimate of the ekf origin height
734 0 : ftype deltaTime = constrain_ftype(0.001f * (imuDataDelayed.time_ms - lastOriginHgtTime_ms), 0.0f, 1.0f);
735 0 : if (activeHgtSource == HGT_SOURCE_BARO) {
736 : // Use the baro drift rate
737 : const ftype baroDriftRate = 0.05f;
738 0 : ekfOriginHgtVar += sq(baroDriftRate * deltaTime);
739 0 : } else if (activeHgtSource == HGT_SOURCE_RNG) {
740 : // use the worse case expected terrain gradient and vehicle horizontal speed
741 : const ftype maxTerrGrad = 0.25f;
742 0 : ekfOriginHgtVar += sq(maxTerrGrad * stateStruct.velocity.xy().length() * deltaTime);
743 : } else {
744 : // by definition our height source is absolute so cannot run this filter
745 : return;
746 : }
747 0 : lastOriginHgtTime_ms = imuDataDelayed.time_ms;
748 :
749 : // calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error
750 : // when not using GPS as height source
751 0 : ftype originHgtObsVar = sq(gpsHgtAccuracy) + P[8][8];
752 :
753 : // calculate the correction gain
754 0 : ftype gain = ekfOriginHgtVar / (ekfOriginHgtVar + originHgtObsVar);
755 :
756 : // calculate the innovation
757 0 : ftype innovation = - stateStruct.position.z - gpsDataDelayed.hgt;
758 :
759 : // check the innovation variance ratio
760 0 : ftype ratio = sq(innovation) / (ekfOriginHgtVar + originHgtObsVar);
761 :
762 : // correct the EKF origin and variance estimate if the innovation is less than 5-sigma
763 0 : if (ratio < 25.0f && gpsAccuracyGood) {
764 0 : ekfGpsRefHgt -= (double)(gain * innovation);
765 0 : ekfOriginHgtVar -= MAX(gain * ekfOriginHgtVar , 0.0f);
766 : }
767 : }
768 :
769 : /********************************************************
770 : * Air Speed Measurements *
771 : ********************************************************/
772 :
773 : // check for new airspeed data and update stored measurements if available
774 3230372 : void NavEKF2_core::readAirSpdData()
775 : {
776 : // if airspeed reading is valid and is set by the user to be used and has been updated then
777 : // we take a new reading, convert from EAS to TAS and set the flag letting other functions
778 : // know a new measurement is available
779 3230372 : const auto *aspeed = dal.airspeed();
780 2968482 : if (aspeed &&
781 2968482 : aspeed->use() &&
782 6046138 : aspeed->healthy() &&
783 2815694 : aspeed->last_update_ms() != timeTasReceived_ms) {
784 556412 : tasDataNew.tas = aspeed->get_airspeed() * dal.get_EAS2TAS();
785 556412 : timeTasReceived_ms = aspeed->last_update_ms();
786 556412 : tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
787 :
788 : // Correct for the average intersampling delay due to the filter update rate
789 556412 : tasDataNew.time_ms -= localFilterTimeStep_ms/2;
790 :
791 : // Save data into the buffer to be fused when the fusion time horizon catches up with it
792 556412 : storedTAS.push(tasDataNew);
793 : }
794 : // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
795 3230372 : tasDataToFuse = storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
796 3230372 : }
797 :
798 : /********************************************************
799 : * Range Beacon Measurements *
800 : ********************************************************/
801 :
802 : #if AP_BEACON_ENABLED
803 : // check for new range beacon data and push to data buffer if available
804 3248540 : void NavEKF2_core::readRngBcnData()
805 : {
806 : // get the location of the beacon data
807 3248540 : const auto *beacon = dal.beacon();
808 :
809 : // exit immediately if no beacon object
810 3248540 : if (beacon == nullptr) {
811 3248540 : return;
812 : }
813 :
814 : // get the number of beacons in use
815 0 : N_beacons = beacon->count();
816 :
817 : // search through all the beacons for new data and if we find it stop searching and push the data into the observation buffer
818 : bool newDataToPush = false;
819 : uint8_t numRngBcnsChecked = 0;
820 : // start the search one index up from where we left it last time
821 0 : uint8_t index = lastRngBcnChecked;
822 0 : while (!newDataToPush && numRngBcnsChecked < N_beacons) {
823 : // track the number of beacons checked
824 0 : numRngBcnsChecked++;
825 :
826 : // move to next beacon, wrap index if necessary
827 0 : index++;
828 0 : if (index >= N_beacons) {
829 : index = 0;
830 : }
831 :
832 : // check that the beacon is healthy and has new data
833 0 : if (beacon->beacon_healthy(index) &&
834 0 : beacon->beacon_last_update_ms(index) != lastTimeRngBcn_ms[index])
835 : {
836 : // set the timestamp, correcting for measurement delay and average intersampling delay due to the filter update rate
837 0 : lastTimeRngBcn_ms[index] = beacon->beacon_last_update_ms(index);
838 0 : rngBcnDataNew.time_ms = lastTimeRngBcn_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2;
839 :
840 : // set the range noise
841 : // TODO the range library should provide the noise/accuracy estimate for each beacon
842 0 : rngBcnDataNew.rngErr = frontend->_rngBcnNoise.get();
843 :
844 : // set the range measurement
845 0 : rngBcnDataNew.rng = beacon->beacon_distance(index);
846 :
847 : // set the beacon position
848 0 : rngBcnDataNew.beacon_posNED = beacon->beacon_position(index).toftype();
849 :
850 : // identify the beacon identifier
851 0 : rngBcnDataNew.beacon_ID = index;
852 :
853 : // indicate we have new data to push to the buffer
854 : newDataToPush = true;
855 :
856 : // update the last checked index
857 0 : lastRngBcnChecked = index;
858 : }
859 : }
860 :
861 : // Check if the beacon system has returned a 3D fix
862 0 : Vector3f beaconVehiclePosNEDF;
863 : float beaconVehiclePosErrF;
864 0 : if (beacon->get_vehicle_position_ned(beaconVehiclePosNEDF, beaconVehiclePosErrF)) {
865 0 : rngBcnLast3DmeasTime_ms = imuSampleTime_ms;
866 : }
867 0 : beaconVehiclePosNED = beaconVehiclePosNEDF.toftype();
868 0 : beaconVehiclePosErr = beaconVehiclePosErrF;
869 :
870 :
871 : // Check if the range beacon data can be used to align the vehicle position
872 0 : if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && beaconVehiclePosErr < 1.0f && rngBcnAlignmentCompleted) {
873 : // check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
874 0 : ftype posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
875 0 : ftype posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
876 0 : if (posDiffSq < 9.0f*posDiffVar) {
877 0 : rngBcnGoodToAlign = true;
878 : // Set the EKF origin and magnetic field declination if not previously set
879 0 : if (!validOrigin && PV_AidingMode != AID_ABSOLUTE) {
880 : // get origin from beacon system
881 0 : Location origin_loc;
882 0 : if (beacon->get_origin(origin_loc)) {
883 0 : setOriginLLH(origin_loc);
884 :
885 : // set the NE earth magnetic field states using the published declination
886 : // and set the corresponding variances and covariances
887 0 : alignMagStateDeclination();
888 :
889 : // Set the uncertainty of the origin height
890 0 : ekfOriginHgtVar = sq(beaconVehiclePosErr);
891 : }
892 : }
893 : } else {
894 0 : rngBcnGoodToAlign = false;
895 : }
896 : } else {
897 0 : rngBcnGoodToAlign = false;
898 : }
899 :
900 : // Save data into the buffer to be fused when the fusion time horizon catches up with it
901 0 : if (newDataToPush) {
902 0 : storedRangeBeacon.push(rngBcnDataNew);
903 : }
904 :
905 : // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
906 0 : rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed,imuDataDelayed.time_ms);
907 :
908 : }
909 : #endif // AP_BEACON_ENABLED
910 :
911 : /*
912 : update timing statistics structure
913 : */
914 3256540 : void NavEKF2_core::updateTimingStatistics(void)
915 : {
916 3256540 : if (timing.count == 0) {
917 12886 : timing.dtIMUavg_max = dtIMUavg;
918 12886 : timing.dtIMUavg_min = dtIMUavg;
919 12886 : timing.dtEKFavg_max = dtEkfAvg;
920 12886 : timing.dtEKFavg_min = dtEkfAvg;
921 12886 : timing.delAngDT_max = imuDataDelayed.delAngDT;
922 12886 : timing.delAngDT_min = imuDataDelayed.delAngDT;
923 12886 : timing.delVelDT_max = imuDataDelayed.delVelDT;
924 12886 : timing.delVelDT_min = imuDataDelayed.delVelDT;
925 : } else {
926 3243654 : timing.dtIMUavg_max = MAX(timing.dtIMUavg_max, dtIMUavg);
927 3243654 : timing.dtIMUavg_min = MIN(timing.dtIMUavg_min, dtIMUavg);
928 3243654 : timing.dtEKFavg_max = MAX(timing.dtEKFavg_max, dtEkfAvg);
929 3243654 : timing.dtEKFavg_min = MIN(timing.dtEKFavg_min, dtEkfAvg);
930 3243654 : timing.delAngDT_max = MAX(timing.delAngDT_max, imuDataDelayed.delAngDT);
931 3243654 : timing.delAngDT_min = MIN(timing.delAngDT_min, imuDataDelayed.delAngDT);
932 3243654 : timing.delVelDT_max = MAX(timing.delVelDT_max, imuDataDelayed.delVelDT);
933 3243654 : timing.delVelDT_min = MIN(timing.delVelDT_min, imuDataDelayed.delVelDT);
934 : }
935 3256540 : timing.count++;
936 3256540 : }
937 :
938 0 : void NavEKF2_core::writeExtNavData(const Vector3f &pos, const Quaternion &quat, float posErr, float angErr, uint32_t timeStamp_ms, uint16_t delay_ms, uint32_t resetTime_ms)
939 : {
940 : // protect against NaN
941 0 : if (pos.is_nan() || isnan(posErr) || quat.is_nan() || isnan(angErr)) {
942 0 : return;
943 : }
944 :
945 : // limit update rate to maximum allowed by sensor buffers and fusion process
946 : // don't try to write to buffer until the filter has been initialised
947 0 : if ((timeStamp_ms - extNavMeasTime_ms) < 20) {
948 : return;
949 : } else {
950 0 : extNavMeasTime_ms = timeStamp_ms;
951 : }
952 :
953 0 : if (resetTime_ms != extNavLastPosResetTime_ms) {
954 0 : extNavDataNew.posReset = true;
955 0 : extNavLastPosResetTime_ms = resetTime_ms;
956 : } else {
957 0 : extNavDataNew.posReset = false;
958 : }
959 :
960 0 : extNavDataNew.pos = pos.toftype();
961 0 : extNavDataNew.quat = quat.toftype();
962 0 : extNavDataNew.posErr = posErr;
963 0 : extNavDataNew.angErr = angErr;
964 0 : timeStamp_ms = timeStamp_ms - delay_ms;
965 : // Correct for the average intersampling delay due to the filter updaterate
966 0 : timeStamp_ms -= localFilterTimeStep_ms/2;
967 : // Prevent time delay exceeding age of oldest IMU data in the buffer
968 0 : timeStamp_ms = MAX(timeStamp_ms,imuDataDelayed.time_ms);
969 0 : extNavDataNew.time_ms = timeStamp_ms;
970 :
971 0 : storedExtNav.push(extNavDataNew);
972 : }
973 :
974 : /*
975 : return declination in radians
976 : */
977 609763 : ftype NavEKF2_core::MagDeclination(void) const
978 : {
979 : // if we are using the WMM tables then use the table declination
980 : // to ensure consistency with the table mag field. Otherwise use
981 : // the declination from the compass library
982 609763 : if (have_table_earth_field && frontend->_mag_ef_limit > 0) {
983 563294 : return table_declination;
984 : }
985 46469 : if (!use_compass()) {
986 : return 0;
987 : }
988 46468 : return dal.compass().get_declination();
989 : }
990 :
991 : /*
992 : update estimates of inactive bias states. This keeps inactive IMUs
993 : as hot-spares so we can switch to them without causing a jump in the
994 : error
995 : */
996 4231300 : void NavEKF2_core::learnInactiveBiases(void)
997 : {
998 : #if INS_MAX_INSTANCES == 1
999 : inactiveBias[0].gyro_bias = stateStruct.gyro_bias;
1000 : inactiveBias[0].gyro_scale = stateStruct.gyro_scale;
1001 : inactiveBias[0].accel_zbias = stateStruct.accel_zbias;
1002 : #else
1003 4231300 : const auto &ins = dal.ins();
1004 :
1005 : // learn gyro biases
1006 16925200 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
1007 12693900 : if (!ins.use_gyro(i)) {
1008 : // can't use this gyro
1009 4179824 : continue;
1010 : }
1011 8514076 : if (gyro_index_active == i) {
1012 : // use current estimates from main filter of gyro bias and scale
1013 4231300 : inactiveBias[i].gyro_bias = stateStruct.gyro_bias;
1014 4231300 : inactiveBias[i].gyro_scale = stateStruct.gyro_scale;
1015 : } else {
1016 : // get filtered gyro and use the difference between the
1017 : // corrected gyro on the active IMU and the inactive IMU
1018 : // to move the inactive bias towards the right value
1019 4282776 : Vector3F filtered_gyro_active = ins.get_gyro(gyro_index_active).toftype() - (stateStruct.gyro_bias/dtEkfAvg);
1020 4282776 : Vector3F filtered_gyro_inactive = ins.get_gyro(i).toftype() - (inactiveBias[i].gyro_bias/dtEkfAvg);
1021 4282776 : Vector3F error = filtered_gyro_active - filtered_gyro_inactive;
1022 :
1023 : // prevent a single large error from contaminating bias estimate
1024 : const ftype bias_limit = radians(5);
1025 4282776 : error.x = constrain_ftype(error.x, -bias_limit, bias_limit);
1026 4282776 : error.y = constrain_ftype(error.y, -bias_limit, bias_limit);
1027 4282776 : error.z = constrain_ftype(error.z, -bias_limit, bias_limit);
1028 :
1029 : // slowly bring the inactive gyro in line with the active gyro. This corrects a 5 deg/sec
1030 : // gyro bias error in around 1 minute
1031 4282776 : inactiveBias[i].gyro_bias -= error * (1.0e-4f * dtEkfAvg);
1032 : }
1033 : }
1034 :
1035 : // learn accel biases
1036 16925200 : for (uint8_t i=0; i<INS_MAX_INSTANCES; i++) {
1037 12693900 : if (!ins.use_accel(i)) {
1038 : // can't use this accel
1039 4179824 : continue;
1040 : }
1041 8514076 : if (accel_index_active == i) {
1042 : // use current estimate from main filter
1043 4231300 : inactiveBias[i].accel_zbias = stateStruct.accel_zbias;
1044 : } else {
1045 : // get filtered accel and use the difference between the
1046 : // corrected accel on the active IMU and the inactive IMU
1047 : // to move the inactive bias towards the right value
1048 4282776 : ftype filtered_accel_active = ins.get_accel(accel_index_active).z - (stateStruct.accel_zbias/dtEkfAvg);
1049 4282776 : ftype filtered_accel_inactive = ins.get_accel(i).z - (inactiveBias[i].accel_zbias/dtEkfAvg);
1050 4282776 : ftype error = filtered_accel_active - filtered_accel_inactive;
1051 :
1052 : // prevent a single large error from contaminating bias estimate
1053 : const ftype bias_limit = 1; // m/s/s
1054 4282776 : error = constrain_ftype(error, -bias_limit, bias_limit);
1055 :
1056 : // slowly bring the inactive accel in line with the active accel
1057 : // this learns 0.5m/s/s bias in about 1 minute
1058 4282776 : inactiveBias[i].accel_zbias -= error * (1.0e-4f * dtEkfAvg);
1059 : }
1060 : }
1061 : #endif
1062 4231300 : }
1063 :
1064 : // Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
1065 0 : void NavEKF2_core::writeDefaultAirSpeed(float airspeed)
1066 : {
1067 0 : defaultAirSpeed = airspeed;
1068 0 : }
1069 :
1070 0 : void NavEKF2_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t timeStamp_ms, uint16_t delay_ms)
1071 : {
1072 : // protect against NaN
1073 0 : if (vel.is_nan() || isnan(err)) {
1074 0 : return;
1075 : }
1076 :
1077 0 : if ((timeStamp_ms - extNavVelMeasTime_ms) < 20) {
1078 : return;
1079 : }
1080 :
1081 0 : extNavVelMeasTime_ms = timeStamp_ms;
1082 0 : useExtNavVel = true;
1083 0 : extNavVelNew.vel = vel.toftype();
1084 0 : extNavVelNew.err = err;
1085 0 : timeStamp_ms = timeStamp_ms - delay_ms;
1086 : // Correct for the average intersampling delay due to the filter updaterate
1087 0 : timeStamp_ms -= localFilterTimeStep_ms/2;
1088 : // Prevent time delay exceeding age of oldest IMU data in the buffer
1089 0 : timeStamp_ms = MAX(timeStamp_ms,imuDataDelayed.time_ms);
1090 0 : extNavVelNew.time_ms = timeStamp_ms;
1091 0 : storedExtNavVel.push(extNavVelNew);
1092 : }
|
