1 #include <AP_HAL/AP_HAL.h>
3 #include "AP_NavEKF3_core.h"
4 #include <GCS_MAVLink/GCS.h>
5 #include <AP_Logger/AP_Logger.h>
6 #include <AP_DAL/AP_DAL.h>
7 #include <AP_InternalError/AP_InternalError.h>
9 /********************************************************
10 * OPT FLOW AND RANGE FINDER *
11 ********************************************************/
13 // Read the range finder and take new measurements if available
14 // Apply a median filter
15 void NavEKF3_core::readRangeFinder(void)
20 // get theoretical correct range when the vehicle is on the ground
21 // don't allow range to go below 5cm because this can cause problems with optical flow processing
22 const auto *_rng
= dal
.rangefinder();
23 if (_rng
== nullptr) {
26 rngOnGnd
= MAX(_rng
->ground_clearance_cm_orient(ROTATION_PITCH_270
) * 0.01f
, 0.05f
);
28 // limit update rate to maximum allowed by data buffers
29 if ((imuSampleTime_ms
- lastRngMeasTime_ms
) > frontend
->sensorIntervalMin_ms
) {
31 // reset the timer used to control the measurement rate
32 lastRngMeasTime_ms
= imuSampleTime_ms
;
34 // store samples and sample time into a ring buffer if valid
35 // use data from two range finders if available
37 for (uint8_t sensorIndex
= 0; sensorIndex
<= 1; sensorIndex
++) {
38 const auto *sensor
= _rng
->get_backend(sensorIndex
);
39 if (sensor
== nullptr) {
42 if ((sensor
->orientation() == ROTATION_PITCH_270
) && (sensor
->status() == AP_DAL_RangeFinder::Status::Good
)) {
43 rngMeasIndex
[sensorIndex
] ++;
44 if (rngMeasIndex
[sensorIndex
] > 2) {
45 rngMeasIndex
[sensorIndex
] = 0;
47 storedRngMeasTime_ms
[sensorIndex
][rngMeasIndex
[sensorIndex
]] = imuSampleTime_ms
- 25;
48 storedRngMeas
[sensorIndex
][rngMeasIndex
[sensorIndex
]] = sensor
->distance_cm() * 0.01f
;
53 // check for three fresh samples
54 bool sampleFresh
[2][3] = {};
55 for (uint8_t index
= 0; index
<= 2; index
++) {
56 sampleFresh
[sensorIndex
][index
] = (imuSampleTime_ms
- storedRngMeasTime_ms
[sensorIndex
][index
]) < 500;
59 // find the median value if we have three fresh samples
60 if (sampleFresh
[sensorIndex
][0] && sampleFresh
[sensorIndex
][1] && sampleFresh
[sensorIndex
][2]) {
61 if (storedRngMeas
[sensorIndex
][0] > storedRngMeas
[sensorIndex
][1]) {
68 if (storedRngMeas
[sensorIndex
][2] > storedRngMeas
[sensorIndex
][maxIndex
]) {
70 } else if (storedRngMeas
[sensorIndex
][2] < storedRngMeas
[sensorIndex
][minIndex
]) {
76 // don't allow time to go backwards
77 if (storedRngMeasTime_ms
[sensorIndex
][midIndex
] > rangeDataNew
.time_ms
) {
78 rangeDataNew
.time_ms
= storedRngMeasTime_ms
[sensorIndex
][midIndex
];
81 // limit the measured range to be no less than the on-ground range
82 rangeDataNew
.rng
= MAX(storedRngMeas
[sensorIndex
][midIndex
],rngOnGnd
);
84 // get position in body frame for the current sensor
85 rangeDataNew
.sensor_idx
= sensorIndex
;
87 // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
88 storedRange
.push(rangeDataNew
);
90 // indicate we have updated the measurement
91 rngValidMeaTime_ms
= imuSampleTime_ms
;
93 } else if (!takeOffDetected
&& ((imuSampleTime_ms
- rngValidMeaTime_ms
) > 200)) {
94 // before takeoff we assume on-ground range value if there is no data
95 rangeDataNew
.time_ms
= imuSampleTime_ms
;
96 rangeDataNew
.rng
= rngOnGnd
;
98 // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
99 storedRange
.push(rangeDataNew
);
101 // indicate we have updated the measurement
102 rngValidMeaTime_ms
= imuSampleTime_ms
;
109 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
)
111 #if EK3_FEATURE_BODY_ODOM
112 // protect against NaN
113 if (isnan(quality
) || delPos
.is_nan() || delAng
.is_nan() || isnan(delTime
) || posOffset
.is_nan()) {
117 // limit update rate to maximum allowed by sensor buffers and fusion process
118 // don't try to write to buffer until the filter has been initialised
119 if (((timeStamp_ms
- bodyOdmMeasTime_ms
) < frontend
->sensorIntervalMin_ms
) || (delTime
< dtEkfAvg
) || !statesInitialised
) {
123 // subtract delay from timestamp
124 timeStamp_ms
-= delay_ms
;
126 bodyOdmDataNew
.body_offset
= posOffset
.toftype();
127 bodyOdmDataNew
.vel
= delPos
.toftype() * (1.0/delTime
);
128 bodyOdmDataNew
.time_ms
= timeStamp_ms
;
129 bodyOdmDataNew
.angRate
= (delAng
* (1.0/delTime
)).toftype();
130 bodyOdmMeasTime_ms
= timeStamp_ms
;
132 // simple model of accuracy
133 // TODO move this calculation outside of EKF into the sensor driver
134 bodyOdmDataNew
.velErr
= frontend
->_visOdmVelErrMin
+ (frontend
->_visOdmVelErrMax
- frontend
->_visOdmVelErrMin
) * (1.0f
- 0.01f
* quality
);
136 storedBodyOdm
.push(bodyOdmDataNew
);
137 #endif // EK3_FEATURE_BODY_ODOM
140 #if EK3_FEATURE_BODY_ODOM
141 void NavEKF3_core::writeWheelOdom(float delAng
, float delTime
, uint32_t timeStamp_ms
, const Vector3f
&posOffset
, float radius
)
143 // This is a simple hack to get wheel encoder data into the EKF and verify the interface sign conventions and units
144 // It uses the exisiting body frame velocity fusion.
145 // TODO implement a dedicated wheel odometry observation model
147 // rate limiting to 50hz should be done by the caller
148 // limit update rate to maximum allowed by sensor buffers and fusion process
149 // don't try to write to buffer until the filter has been initialised
150 if ((delTime
< dtEkfAvg
) || !statesInitialised
) {
154 wheel_odm_elements wheelOdmDataNew
= {};
155 wheelOdmDataNew
.hub_offset
= posOffset
.toftype();
156 wheelOdmDataNew
.delAng
= delAng
;
157 wheelOdmDataNew
.radius
= radius
;
158 wheelOdmDataNew
.delTime
= delTime
;
160 // because we are currently converting to an equivalent velocity measurement before fusing
161 // the measurement time is moved back to the middle of the sampling period
162 wheelOdmDataNew
.time_ms
= timeStamp_ms
- (uint32_t)(500.0f
* delTime
);
164 storedWheelOdm
.push(wheelOdmDataNew
);
166 #endif // EK3_FEATURE_BODY_ODOM
168 // write the raw optical flow measurements
169 // this needs to be called externally.
170 void NavEKF3_core::writeOptFlowMeas(const uint8_t rawFlowQuality
, const Vector2f
&rawFlowRates
, const Vector2f
&rawGyroRates
, const uint32_t msecFlowMeas
, const Vector3f
&posOffset
)
172 // limit update rate to maximum allowed by sensor buffers
173 if ((imuSampleTime_ms
- flowMeaTime_ms
) < frontend
->sensorIntervalMin_ms
) {
177 // The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
178 // The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
179 // A positive X rate is produced by a positive sensor rotation about the X axis
180 // A positive Y rate is produced by a positive sensor rotation about the Y axis
181 // This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
182 // 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
183 flowMeaTime_ms
= imuSampleTime_ms
;
184 // calculate bias errors on flow sensor gyro rates, but protect against spikes in data
185 // reset the accumulated body delta angle and time
186 // don't do the calculation if not enough time lapsed for a reliable body rate measurement
187 if (delTimeOF
> 0.01f
) {
188 flowGyroBias
.x
= 0.99f
* flowGyroBias
.x
+ 0.01f
* constrain_ftype((rawGyroRates
.x
- delAngBodyOF
.x
/delTimeOF
),-0.1f
,0.1f
);
189 flowGyroBias
.y
= 0.99f
* flowGyroBias
.y
+ 0.01f
* constrain_ftype((rawGyroRates
.y
- delAngBodyOF
.y
/delTimeOF
),-0.1f
,0.1f
);
193 // by definition if this function is called, then flow measurements have been provided so we
194 // need to run the optical flow takeoff detection
195 detectOptFlowTakeoff();
197 // don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
198 if ((rawFlowQuality
> 0) && rawFlowRates
.length() < 4.2f
&& rawGyroRates
.length() < 4.2f
) {
199 // correct flow sensor body rates for bias and write
200 of_elements ofDataNew
{};
201 ofDataNew
.bodyRadXYZ
.x
= rawGyroRates
.x
- flowGyroBias
.x
;
202 ofDataNew
.bodyRadXYZ
.y
= rawGyroRates
.y
- flowGyroBias
.y
;
203 // the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
204 if (delTimeOF
> 0.001f
) {
205 // first preference is to use the rate averaged over the same sampling period as the flow sensor
206 ofDataNew
.bodyRadXYZ
.z
= delAngBodyOF
.z
/ delTimeOF
;
207 } else if (imuDataNew
.delAngDT
> 0.001f
){
208 // second preference is to use most recent IMU data
209 ofDataNew
.bodyRadXYZ
.z
= imuDataNew
.delAng
.z
/ imuDataNew
.delAngDT
;
211 // third preference is use zero
212 ofDataNew
.bodyRadXYZ
.z
= 0.0f
;
214 // write uncorrected flow rate measurements
215 // note correction for different axis and sign conventions used by the px4flow sensor
216 ofDataNew
.flowRadXY
= - rawFlowRates
.toftype(); // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
217 // write the flow sensor position in body frame
218 ofDataNew
.body_offset
= posOffset
.toftype();
219 // write flow rate measurements corrected for body rates
220 ofDataNew
.flowRadXYcomp
.x
= ofDataNew
.flowRadXY
.x
+ ofDataNew
.bodyRadXYZ
.x
;
221 ofDataNew
.flowRadXYcomp
.y
= ofDataNew
.flowRadXY
.y
+ ofDataNew
.bodyRadXYZ
.y
;
222 // record time last observation was received so we can detect loss of data elsewhere
223 flowValidMeaTime_ms
= imuSampleTime_ms
;
224 // estimate sample time of the measurement
225 ofDataNew
.time_ms
= imuSampleTime_ms
- frontend
->_flowDelay_ms
- frontend
->flowTimeDeltaAvg_ms
/2;
226 // Correct for the average intersampling delay due to the filter updaterate
227 ofDataNew
.time_ms
-= localFilterTimeStep_ms
/2;
228 // Prevent time delay exceeding age of oldest IMU data in the buffer
229 ofDataNew
.time_ms
= MAX(ofDataNew
.time_ms
,imuDataDelayed
.time_ms
);
230 // Save data to buffer
231 storedOF
.push(ofDataNew
);
236 /********************************************************
238 ********************************************************/
240 // try changing compass, return true if a new compass is found
241 void NavEKF3_core::tryChangeCompass(void)
243 const auto &compass
= dal
.compass();
244 const uint8_t maxCount
= compass
.get_count();
246 // search through the list of magnetometers
247 for (uint8_t i
=1; i
<maxCount
; i
++) {
248 uint8_t tempIndex
= magSelectIndex
+ i
;
249 // loop back to the start index if we have exceeded the bounds
250 if (tempIndex
>= maxCount
) {
251 tempIndex
-= maxCount
;
253 // if the magnetometer is allowed to be used for yaw and has a different index, we start using it
254 if (compass
.healthy(tempIndex
) && compass
.use_for_yaw(tempIndex
) && tempIndex
!= magSelectIndex
) {
255 magSelectIndex
= tempIndex
;
256 GCS_SEND_TEXT(MAV_SEVERITY_INFO
, "EKF3 IMU%u switching to compass %u",(unsigned)imu_index
,magSelectIndex
);
257 // reset the timeout flag and timer
259 lastHealthyMagTime_ms
= imuSampleTime_ms
;
260 // zero the learned magnetometer bias states
261 stateStruct
.body_magfield
.zero();
262 // clear the measurement buffer
264 // clear the data waiting flag so that we do not use any data pending from the previous sensor
265 magDataToFuse
= false;
266 // request a reset of the magnetic field states
267 magStateResetRequest
= true;
268 // declare the field unlearned so that the reset request will be obeyed
269 magFieldLearned
= false;
270 // reset body mag variances on next CovariancePrediction
271 needMagBodyVarReset
= true;
277 // check for new magnetometer data and update store measurements if available
278 void NavEKF3_core::readMagData()
280 const auto &compass
= dal
.compass();
282 if (!compass
.available()) {
283 allMagSensorsFailed
= true;
287 // If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
288 // magnetometer, then declare the magnetometers as failed for this flight
289 const uint8_t maxCount
= compass
.get_count();
290 if (allMagSensorsFailed
|| (magTimeout
&& assume_zero_sideslip() && magSelectIndex
>= maxCount
-1 && inFlight
)) {
291 allMagSensorsFailed
= true;
295 if (compass
.learn_offsets_enabled()) {
296 // while learning offsets keep all mag states reset
297 InitialiseVariablesMag();
298 wasLearningCompass_ms
= imuSampleTime_ms
;
299 } else if (wasLearningCompass_ms
!= 0 && imuSampleTime_ms
- wasLearningCompass_ms
> 1000) {
300 wasLearningCompass_ms
= 0;
301 // force a new yaw alignment 1s after learning completes. The
302 // delay is to ensure any buffered mag samples are discarded
303 yawAlignComplete
= false;
304 InitialiseVariablesMag();
307 // If the magnetometer has timed out (been rejected for too long), we find another magnetometer to use if available
308 // 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
309 // 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
310 // if the timeout is due to a sensor failure, then declare a timeout regardless of onground status
312 bool fusionTimeout
= magTimeout
&& !onGround
&& imuSampleTime_ms
- ekfStartTime_ms
> 30000 && !(frontend
->_affinity
& EKF_AFFINITY_MAG
);
313 bool sensorTimeout
= !compass
.healthy(magSelectIndex
) && imuSampleTime_ms
- lastMagRead_ms
> frontend
->magFailTimeLimit_ms
;
314 if (fusionTimeout
|| sensorTimeout
) {
319 // limit compass update rate to prevent high processor loading because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
321 compass
.healthy(magSelectIndex
) &&
322 ((compass
.last_update_usec(magSelectIndex
) - lastMagUpdate_us
) > 1000 * frontend
->sensorIntervalMin_ms
)) {
324 // detect changes to magnetometer offset parameters and reset states
325 Vector3F nowMagOffsets
= compass
.get_offsets(magSelectIndex
).toftype();
326 bool changeDetected
= lastMagOffsetsValid
&& (nowMagOffsets
!= lastMagOffsets
);
327 if (changeDetected
) {
328 // zero the learned magnetometer bias states
329 stateStruct
.body_magfield
.zero();
330 // clear the measurement buffer
333 // reset body mag variances on next
334 // CovariancePrediction. This copes with possible errors
335 // in the new offsets
336 needMagBodyVarReset
= true;
338 lastMagOffsets
= nowMagOffsets
;
339 lastMagOffsetsValid
= true;
341 // store time of last measurement update
342 lastMagUpdate_us
= compass
.last_update_usec(magSelectIndex
);
344 // Magnetometer data at the current time horizon
345 mag_elements magDataNew
;
347 // estimate of time magnetometer measurement was taken, allowing for delays
348 magDataNew
.time_ms
= imuSampleTime_ms
- frontend
->magDelay_ms
;
350 // Correct for the average intersampling delay due to the filter updaterate
351 magDataNew
.time_ms
-= localFilterTimeStep_ms
/2;
353 // read compass data and scale to improve numerical conditioning
354 magDataNew
.mag
= (compass
.get_field(magSelectIndex
) * 0.001f
).toftype();
356 // check for consistent data between magnetometers
357 consistentMagData
= compass
.consistent();
359 // save magnetometer measurement to buffer to be fused later
360 storedMag
.push(magDataNew
);
362 // remember time we read compass, to detect compass sensor failure
363 lastMagRead_ms
= imuSampleTime_ms
;
367 /********************************************************
368 * Inertial Measurements *
369 ********************************************************/
372 * Read IMU delta angle and delta velocity measurements and downsample to 100Hz
373 * for storage in the data buffers used by the EKF. If the IMU data arrives at
374 * lower rate than 100Hz, then no downsampling or upsampling will be performed.
375 * Downsampling is done using a method that does not introduce coning or sculling
378 void NavEKF3_core::readIMUData()
380 const auto &ins
= dal
.ins();
382 // calculate an averaged IMU update rate using a spike and lowpass filter combination
383 dtIMUavg
= 0.02f
* constrain_ftype(ins
.get_loop_delta_t(),0.5f
* dtIMUavg
, 2.0f
* dtIMUavg
) + 0.98f
* dtIMUavg
;
385 // the imu sample time is used as a common time reference throughout the filter
386 imuSampleTime_ms
= frontend
->imuSampleTime_us
/ 1000;
388 uint8_t accel_active
, gyro_active
;
390 if (ins
.use_accel(imu_index
)) {
391 accel_active
= imu_index
;
393 accel_active
= ins
.get_primary_accel();
396 if (ins
.use_gyro(imu_index
)) {
397 gyro_active
= imu_index
;
399 gyro_active
= ins
.get_primary_gyro();
402 if (gyro_active
!= gyro_index_active
) {
403 // we are switching active gyro at runtime. Copy over the
404 // bias we have learned from the previously inactive
405 // gyro. We don't re-init the bias uncertainty as it should
406 // have the same uncertainty as the previously active gyro
407 stateStruct
.gyro_bias
= inactiveBias
[gyro_active
].gyro_bias
;
408 gyro_index_active
= gyro_active
;
411 if (accel_active
!= accel_index_active
) {
412 // switch to the learned accel bias for this IMU
413 stateStruct
.accel_bias
= inactiveBias
[accel_active
].accel_bias
;
414 accel_index_active
= accel_active
;
417 // update the inactive bias states
418 learnInactiveBiases();
420 // run movement check using IMU data
421 updateMovementCheck();
423 readDeltaVelocity(accel_index_active
, imuDataNew
.delVel
, imuDataNew
.delVelDT
);
424 accelPosOffset
= ins
.get_imu_pos_offset(accel_index_active
).toftype();
425 imuDataNew
.accel_index
= accel_index_active
;
427 // Get delta angle data from primary gyro or primary if not available
428 readDeltaAngle(gyro_index_active
, imuDataNew
.delAng
, imuDataNew
.delAngDT
);
429 imuDataNew
.delAngDT
= MAX(imuDataNew
.delAngDT
, 1.0e-4f
);
430 imuDataNew
.gyro_index
= gyro_index_active
;
432 // Get current time stamp
433 imuDataNew
.time_ms
= imuSampleTime_ms
;
435 // Accumulate the measurement time interval for the delta velocity and angle data
436 imuDataDownSampledNew
.delAngDT
+= imuDataNew
.delAngDT
;
437 imuDataDownSampledNew
.delVelDT
+= imuDataNew
.delVelDT
;
439 // use the most recent IMU index for the downsampled IMU
440 // data. This isn't strictly correct if we switch IMUs between
442 imuDataDownSampledNew
.gyro_index
= imuDataNew
.gyro_index
;
443 imuDataDownSampledNew
.accel_index
= imuDataNew
.accel_index
;
445 // Rotate quaternon atitude from previous to new and normalise.
446 // Accumulation using quaternions prevents introduction of coning errors due to downsampling
447 imuQuatDownSampleNew
.rotate(imuDataNew
.delAng
);
448 imuQuatDownSampleNew
.normalize();
450 // Rotate the latest delta velocity into body frame at the start of accumulation
451 Matrix3F deltaRotMat
;
452 imuQuatDownSampleNew
.rotation_matrix(deltaRotMat
);
454 // Apply the delta velocity to the delta velocity accumulator
455 imuDataDownSampledNew
.delVel
+= deltaRotMat
*imuDataNew
.delVel
;
457 // Keep track of the number of IMU frames since the last state prediction
458 framesSincePredict
++;
461 * If the target EKF time step has been accumulated, and the frontend has allowed start of a new predict cycle,
462 * then store the accumulated IMU data to be used by the state prediction, ignoring the frontend permission if more
463 * than twice the target time has lapsed. Adjust the target EKF step time threshold to allow for timing jitter in the
466 if ((imuDataDownSampledNew
.delAngDT
>= (EKF_TARGET_DT
-(dtIMUavg
*0.5f
)) && startPredictEnabled
) ||
467 (imuDataDownSampledNew
.delAngDT
>= 2.0f
*EKF_TARGET_DT
)) {
469 // convert the accumulated quaternion to an equivalent delta angle
470 imuQuatDownSampleNew
.to_axis_angle(imuDataDownSampledNew
.delAng
);
472 // Time stamp the data
473 imuDataDownSampledNew
.time_ms
= imuSampleTime_ms
;
475 // Write data to the FIFO IMU buffer
476 storedIMU
.push_youngest_element(imuDataDownSampledNew
);
478 // calculate the achieved average time step rate for the EKF using a combination spike and LPF
479 ftype dtNow
= constrain_ftype(0.5f
*(imuDataDownSampledNew
.delAngDT
+imuDataDownSampledNew
.delVelDT
),0.5f
* dtEkfAvg
, 2.0f
* dtEkfAvg
);
480 dtEkfAvg
= 0.98f
* dtEkfAvg
+ 0.02f
* dtNow
;
482 // do an addtional down sampling for data used to sample XY body frame drag specific forces
483 SampleDragData(imuDataDownSampledNew
);
485 // zero the accumulated IMU data and quaternion
486 imuDataDownSampledNew
.delAng
.zero();
487 imuDataDownSampledNew
.delVel
.zero();
488 imuDataDownSampledNew
.delAngDT
= 0.0f
;
489 imuDataDownSampledNew
.delVelDT
= 0.0f
;
490 imuQuatDownSampleNew
[0] = 1.0f
;
491 imuQuatDownSampleNew
[3] = imuQuatDownSampleNew
[2] = imuQuatDownSampleNew
[1] = 0.0f
;
493 // reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle
494 framesSincePredict
= 0;
496 // set the flag to let the filter know it has new IMU data and needs to run
499 // extract the oldest available data from the FIFO buffer
500 imuDataDelayed
= storedIMU
.get_oldest_element();
502 // protect against delta time going to zero
503 ftype minDT
= 0.1f
* dtEkfAvg
;
504 imuDataDelayed
.delAngDT
= MAX(imuDataDelayed
.delAngDT
,minDT
);
505 imuDataDelayed
.delVelDT
= MAX(imuDataDelayed
.delVelDT
,minDT
);
507 updateTimingStatistics();
509 // correct the extracted IMU data for sensor errors
510 delAngCorrected
= imuDataDelayed
.delAng
;
511 delVelCorrected
= imuDataDelayed
.delVel
;
512 correctDeltaAngle(delAngCorrected
, imuDataDelayed
.delAngDT
, imuDataDelayed
.gyro_index
);
513 correctDeltaVelocity(delVelCorrected
, imuDataDelayed
.delVelDT
, imuDataDelayed
.accel_index
);
516 // we don't have new IMU data in the buffer so don't run filter updates on this time step
521 // read the delta velocity and corresponding time interval from the IMU
522 // return false if data is not available
523 bool NavEKF3_core::readDeltaVelocity(uint8_t ins_index
, Vector3F
&dVel
, ftype
&dVel_dt
) {
524 const auto &ins
= dal
.ins();
526 if (ins_index
< ins
.get_accel_count()) {
529 ins
.get_delta_velocity(ins_index
, dVelF
, dVel_dtF
);
530 dVel
= dVelF
.toftype();
532 dVel_dt
= MAX(dVel_dt
,1.0e-4);
538 /********************************************************
539 * Global Position Measurement *
540 ********************************************************/
542 // check for new valid GPS data and update stored measurement if available
543 void NavEKF3_core::readGpsData()
545 // check for new GPS data
546 const auto &gps
= dal
.gps();
548 // limit update rate to avoid overflowing the FIFO buffer
549 if (gps
.last_message_time_ms(selected_gps
) - lastTimeGpsReceived_ms
<= frontend
->sensorIntervalMin_ms
) {
553 if (gps
.status(selected_gps
) < AP_DAL_GPS::GPS_OK_FIX_3D
) {
554 // report GPS fix status
555 gpsCheckStatus
.bad_fix
= true;
556 dal
.snprintf(prearm_fail_string
, sizeof(prearm_fail_string
), "Waiting for 3D fix");
560 // report GPS fix status
561 gpsCheckStatus
.bad_fix
= false;
563 // store fix time from previous read
564 const uint32_t secondLastGpsTime_ms
= lastTimeGpsReceived_ms
;
566 // get current fix time
567 lastTimeGpsReceived_ms
= gps
.last_message_time_ms(selected_gps
);
569 // estimate when the GPS fix was valid, allowing for GPS processing and other delays
570 // ideally we should be using a timing signal from the GPS receiver to set this time
571 // Use the driver specified delay
572 float gps_delay_sec
= 0;
573 gps
.get_lag(selected_gps
, gps_delay_sec
);
574 gpsDataNew
.time_ms
= lastTimeGpsReceived_ms
- (uint32_t)(gps_delay_sec
* 1000.0f
);
576 // Correct for the average intersampling delay due to the filter updaterate
577 gpsDataNew
.time_ms
-= localFilterTimeStep_ms
/2;
579 // Prevent the time stamp falling outside the oldest and newest IMU data in the buffer
580 gpsDataNew
.time_ms
= MIN(MAX(gpsDataNew
.time_ms
,imuDataDelayed
.time_ms
),imuDataDownSampledNew
.time_ms
);
582 // Get which GPS we are using for position information
583 gpsDataNew
.sensor_idx
= selected_gps
;
585 // read the NED velocity from the GPS
586 gpsDataNew
.vel
= gps
.velocity(selected_gps
).toftype();
587 gpsDataNew
.have_vz
= gps
.have_vertical_velocity(selected_gps
);
589 // position and velocity are not yet corrected for sensor position
590 gpsDataNew
.corrected
= false;
592 // Use the speed and position accuracy from the GPS if available, otherwise set it to zero.
593 // Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data
594 ftype alpha
= constrain_ftype(0.0002f
* (lastTimeGpsReceived_ms
- secondLastGpsTime_ms
),0.0f
,1.0f
);
595 gpsSpdAccuracy
*= (1.0f
- alpha
);
597 if (!gps
.speed_accuracy(selected_gps
, gpsSpdAccRaw
)) {
598 gpsSpdAccuracy
= 0.0f
;
600 gpsSpdAccuracy
= MAX(gpsSpdAccuracy
,gpsSpdAccRaw
);
601 gpsSpdAccuracy
= MIN(gpsSpdAccuracy
,50.0f
);
602 gpsSpdAccuracy
= MAX(gpsSpdAccuracy
,frontend
->_gpsHorizVelNoise
);
604 gpsPosAccuracy
*= (1.0f
- alpha
);
606 if (!gps
.horizontal_accuracy(selected_gps
, gpsPosAccRaw
)) {
607 gpsPosAccuracy
= 0.0f
;
609 gpsPosAccuracy
= MAX(gpsPosAccuracy
,gpsPosAccRaw
);
610 gpsPosAccuracy
= MIN(gpsPosAccuracy
,100.0f
);
611 gpsPosAccuracy
= MAX(gpsPosAccuracy
, frontend
->_gpsHorizPosNoise
);
613 gpsHgtAccuracy
*= (1.0f
- alpha
);
615 if (!gps
.vertical_accuracy(selected_gps
, gpsHgtAccRaw
)) {
616 gpsHgtAccuracy
= 0.0f
;
618 gpsHgtAccuracy
= MAX(gpsHgtAccuracy
,gpsHgtAccRaw
);
619 gpsHgtAccuracy
= MIN(gpsHgtAccuracy
,100.0f
);
620 gpsHgtAccuracy
= MAX(gpsHgtAccuracy
, 1.5f
* frontend
->_gpsHorizPosNoise
);
623 // check if we have enough GPS satellites and increase the gps noise scaler if we don't
624 if (gps
.num_sats(selected_gps
) >= 6 && (PV_AidingMode
== AID_ABSOLUTE
)) {
625 gpsNoiseScaler
= 1.0f
;
626 } else if (gps
.num_sats(selected_gps
) == 5 && (PV_AidingMode
== AID_ABSOLUTE
)) {
627 gpsNoiseScaler
= 1.4f
;
628 } else { // <= 4 satellites or in constant position mode
629 gpsNoiseScaler
= 2.0f
;
632 // Check if GPS can output vertical velocity, vertical velocity use is permitted and set GPS fusion mode accordingly
633 if (gpsDataNew
.have_vz
&& frontend
->sources
.useVelZSource(AP_NavEKF_Source::SourceZ::GPS
)) {
634 useGpsVertVel
= true;
636 useGpsVertVel
= false;
639 // Monitor quality of the GPS velocity data before and after alignment
640 calcGpsGoodToAlign();
642 // Post-alignment checks
643 calcGpsGoodForFlight();
645 // Read the GPS location in WGS-84 lat,long,height coordinates
646 const struct Location
&gpsloc
= gps
.location(selected_gps
);
648 // Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
649 if (gpsGoodToAlign
&& !validOrigin
) {
650 Location gpsloc_fieldelevation
= gpsloc
;
651 // if flying, correct for height change from takeoff so that the origin is at field elevation
653 gpsloc_fieldelevation
.alt
+= (int32_t)(100.0f
* stateStruct
.position
.z
);
656 if (!setOrigin(gpsloc_fieldelevation
)) {
657 // set an error as an attempt was made to set the origin more than once
658 INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control
);
662 // set the NE earth magnetic field states using the published declination
663 // and set the corresponding variances and covariances
664 alignMagStateDeclination();
666 // Set the height of the NED origin
667 ekfGpsRefHgt
= (double)0.01 * (double)gpsloc
.alt
+ (double)outputDataNew
.position
.z
;
669 // Set the uncertainty of the GPS origin height
670 ekfOriginHgtVar
= sq(gpsHgtAccuracy
);
674 if (gpsGoodToAlign
&& !have_table_earth_field
) {
675 const auto &compass
= dal
.compass();
676 if (compass
.have_scale_factor(magSelectIndex
) &&
677 compass
.auto_declination_enabled()) {
678 getEarthFieldTable(gpsloc
);
679 if (frontend
->_mag_ef_limit
> 0) {
680 // initialise earth field from tables
681 stateStruct
.earth_magfield
= table_earth_field_ga
;
686 // convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
688 gpsDataNew
.lat
= gpsloc
.lat
;
689 gpsDataNew
.lng
= gpsloc
.lng
;
690 if ((frontend
->_originHgtMode
& (1<<2)) == 0) {
691 gpsDataNew
.hgt
= (ftype
)((double)0.01 * (double)gpsloc
.alt
- ekfGpsRefHgt
);
693 gpsDataNew
.hgt
= 0.01 * (gpsloc
.alt
- EKF_origin
.alt
);
695 storedGPS
.push(gpsDataNew
);
696 // declare GPS in use
701 // check for new valid GPS yaw data
702 void NavEKF3_core::readGpsYawData()
704 const auto &gps
= dal
.gps();
706 // if the GPS has yaw data then fuse it as an Euler yaw angle
707 float yaw_deg
, yaw_accuracy_deg
;
708 uint32_t yaw_time_ms
;
709 if (gps
.status(selected_gps
) >= AP_DAL_GPS::GPS_OK_FIX_3D
&&
710 dal
.gps().gps_yaw_deg(selected_gps
, yaw_deg
, yaw_accuracy_deg
, yaw_time_ms
) &&
711 yaw_time_ms
!= yawMeasTime_ms
) {
712 // GPS modules are rather too optimistic about their
713 // accuracy. Set to min of 5 degrees here to prevent
714 // the user constantly receiving warnings about high
715 // normalised yaw innovations
716 const ftype min_yaw_accuracy_deg
= 5.0f
;
717 yaw_accuracy_deg
= MAX(yaw_accuracy_deg
, min_yaw_accuracy_deg
);
718 writeEulerYawAngle(radians(yaw_deg
), radians(yaw_accuracy_deg
), yaw_time_ms
, 2);
722 // read the delta angle and corresponding time interval from the IMU
723 // return false if data is not available
724 bool NavEKF3_core::readDeltaAngle(uint8_t ins_index
, Vector3F
&dAng
, ftype
&dAngDT
) {
725 const auto &ins
= dal
.ins();
727 if (ins_index
< ins
.get_gyro_count()) {
730 ins
.get_delta_angle(ins_index
, dAngF
, dAngDTF
);
731 dAng
= dAngF
.toftype();
739 /********************************************************
740 * Height Measurements *
741 ********************************************************/
743 // check for new pressure altitude measurement data and update stored measurement if available
744 void NavEKF3_core::readBaroData()
746 // check to see if baro measurement has changed so we know if a new measurement has arrived
747 // limit update rate to avoid overflowing the FIFO buffer
748 const auto &baro
= dal
.baro();
749 if (baro
.get_last_update(selected_baro
) - lastBaroReceived_ms
> frontend
->sensorIntervalMin_ms
) {
751 baroDataNew
.hgt
= baro
.get_altitude(selected_baro
);
753 // time stamp used to check for new measurement
754 lastBaroReceived_ms
= baro
.get_last_update(selected_baro
);
756 // estimate of time height measurement was taken, allowing for delays
757 baroDataNew
.time_ms
= lastBaroReceived_ms
- frontend
->_hgtDelay_ms
;
759 // Correct for the average intersampling delay due to the filter updaterate
760 baroDataNew
.time_ms
-= localFilterTimeStep_ms
/2;
762 // Prevent time delay exceeding age of oldest IMU data in the buffer
763 baroDataNew
.time_ms
= MAX(baroDataNew
.time_ms
,imuDataDelayed
.time_ms
);
765 // save baro measurement to buffer to be fused later
766 storedBaro
.push(baroDataNew
);
770 // calculate filtered offset between baro height measurement and EKF height estimate
771 // offset should be subtracted from baro measurement to match filter estimate
772 // offset is used to enable reversion to baro from alternate height data source
773 void NavEKF3_core::calcFiltBaroOffset()
775 // Apply a first order LPF with spike protection
776 baroHgtOffset
+= 0.1f
* constrain_ftype(baroDataDelayed
.hgt
+ stateStruct
.position
.z
- baroHgtOffset
, -5.0f
, 5.0f
);
779 // correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
780 void NavEKF3_core::correctEkfOriginHeight()
782 // Estimate the WGS-84 height of the EKF's origin using a Bayes filter
784 // calculate the variance of our a-priori estimate of the ekf origin height
785 ftype deltaTime
= constrain_ftype(0.001f
* (imuDataDelayed
.time_ms
- lastOriginHgtTime_ms
), 0.0, 1.0);
786 if (activeHgtSource
== AP_NavEKF_Source::SourceZ::BARO
) {
787 // Use the baro drift rate
788 const ftype baroDriftRate
= 0.05;
789 ekfOriginHgtVar
+= sq(baroDriftRate
* deltaTime
);
790 } else if (activeHgtSource
== AP_NavEKF_Source::SourceZ::RANGEFINDER
) {
791 // use the worse case expected terrain gradient and vehicle horizontal speed
792 const ftype maxTerrGrad
= 0.25;
793 ekfOriginHgtVar
+= sq(maxTerrGrad
* stateStruct
.velocity
.xy().length() * deltaTime
);
795 // by definition our height source is absolute so cannot run this filter
798 lastOriginHgtTime_ms
= imuDataDelayed
.time_ms
;
800 // calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error
801 // when not using GPS as height source
802 ftype originHgtObsVar
= sq(gpsHgtAccuracy
) + P
[9][9];
804 // calculate the correction gain
805 ftype gain
= ekfOriginHgtVar
/ (ekfOriginHgtVar
+ originHgtObsVar
);
807 // calculate the innovation
808 ftype innovation
= - stateStruct
.position
.z
- gpsDataDelayed
.hgt
;
810 // check the innovation variance ratio
811 ftype ratio
= sq(innovation
) / (ekfOriginHgtVar
+ originHgtObsVar
);
813 // correct the EKF origin and variance estimate if the innovation is less than 5-sigma
814 if (ratio
< 25.0f
&& gpsAccuracyGood
) {
815 ekfGpsRefHgt
-= (double)(gain
* innovation
);
816 ekfOriginHgtVar
-= MAX(gain
* ekfOriginHgtVar
, 0.0f
);
820 /********************************************************
821 * Air Speed Measurements *
822 ********************************************************/
824 // check for new airspeed data and update stored measurements if available
825 void NavEKF3_core::readAirSpdData()
827 const float EAS2TAS
= dal
.get_EAS2TAS();
828 // if airspeed reading is valid and is set by the user to be used and has been updated then
829 // we take a new reading, convert from EAS to TAS and set the flag letting other functions
830 // know a new measurement is available
832 const auto *airspeed
= dal
.airspeed();
834 (airspeed
->last_update_ms(selected_airspeed
) - timeTasReceived_ms
) > frontend
->sensorIntervalMin_ms
) {
835 tasDataNew
.tas
= airspeed
->get_airspeed(selected_airspeed
) * EAS2TAS
;
836 timeTasReceived_ms
= airspeed
->last_update_ms(selected_airspeed
);
837 tasDataNew
.time_ms
= timeTasReceived_ms
- frontend
->tasDelay_ms
;
838 tasDataNew
.tasVariance
= sq(MAX(frontend
->_easNoise
* EAS2TAS
, 0.5f
));
839 tasDataNew
.allowFusion
= airspeed
->healthy(selected_airspeed
) && airspeed
->use(selected_airspeed
);
841 // Correct for the average intersampling delay due to the filter update rate
842 tasDataNew
.time_ms
-= localFilterTimeStep_ms
/2;
844 // Save data into the buffer to be fused when the fusion time horizon catches up with it
845 storedTAS
.push(tasDataNew
);
848 // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
849 tasDataToFuse
= storedTAS
.recall(tasDataDelayed
,imuDataDelayed
.time_ms
);
851 float easErrVar
= sq(MAX(frontend
->_easNoise
, 0.5f
));
852 // Allow use of a default value if enabled
853 if (!useAirspeed() &&
854 imuDataDelayed
.time_ms
- tasDataDelayed
.time_ms
> 200 &&
855 is_positive(defaultAirSpeed
)) {
856 tasDataDelayed
.tas
= defaultAirSpeed
* EAS2TAS
;
857 tasDataDelayed
.tasVariance
= sq(MAX(defaultAirSpeedVariance
, easErrVar
));
858 tasDataDelayed
.allowFusion
= true;
859 tasDataDelayed
.time_ms
= 0;
860 usingDefaultAirspeed
= true;
862 usingDefaultAirspeed
= false;
866 /********************************************************
867 * Range Beacon Measurements *
868 ********************************************************/
870 // check for new range beacon data and push to data buffer if available
871 void NavEKF3_core::readRngBcnData()
873 // check that arrays are large enough
874 static_assert(ARRAY_SIZE(lastTimeRngBcn_ms
) >= AP_BEACON_MAX_BEACONS
, "lastTimeRngBcn_ms should have at least AP_BEACON_MAX_BEACONS elements");
876 // get the location of the beacon data
877 const AP_DAL_Beacon
*beacon
= dal
.beacon();
879 // exit immediately if no beacon object
880 if (beacon
== nullptr) {
884 // get the number of beacons in use
885 N_beacons
= MIN(beacon
->count(), ARRAY_SIZE(lastTimeRngBcn_ms
));
887 // search through all the beacons for new data and if we find it stop searching and push the data into the observation buffer
888 bool newDataPushed
= false;
889 uint8_t numRngBcnsChecked
= 0;
890 // start the search one index up from where we left it last time
891 uint8_t index
= lastRngBcnChecked
;
892 while (!newDataPushed
&& (numRngBcnsChecked
< N_beacons
)) {
893 // track the number of beacons checked
896 // move to next beacon, wrap index if necessary
898 if (index
>= N_beacons
) {
902 // check that the beacon is healthy and has new data
903 if (beacon
->beacon_healthy(index
) && beacon
->beacon_last_update_ms(index
) != lastTimeRngBcn_ms
[index
]) {
904 rng_bcn_elements rngBcnDataNew
= {};
906 // set the timestamp, correcting for measurement delay and average intersampling delay due to the filter update rate
907 lastTimeRngBcn_ms
[index
] = beacon
->beacon_last_update_ms(index
);
908 rngBcnDataNew
.time_ms
= lastTimeRngBcn_ms
[index
] - frontend
->_rngBcnDelay_ms
- localFilterTimeStep_ms
/2;
910 // set the range noise
911 // TODO the range library should provide the noise/accuracy estimate for each beacon
912 rngBcnDataNew
.rngErr
= frontend
->_rngBcnNoise
;
914 // set the range measurement
915 rngBcnDataNew
.rng
= beacon
->beacon_distance(index
);
917 // set the beacon position
918 rngBcnDataNew
.beacon_posNED
= beacon
->beacon_position(index
).toftype();
920 // identify the beacon identifier
921 rngBcnDataNew
.beacon_ID
= index
;
923 // indicate we have new data to push to the buffer
924 newDataPushed
= true;
926 // update the last checked index
927 lastRngBcnChecked
= index
;
929 // Save data into the buffer to be fused when the fusion time horizon catches up with it
930 storedRangeBeacon
.push(rngBcnDataNew
);
934 // Check if the beacon system has returned a 3D fix
937 if (beacon
->get_vehicle_position_ned(bp
, bperr
)) {
938 rngBcnLast3DmeasTime_ms
= imuSampleTime_ms
;
940 beaconVehiclePosNED
= bp
.toftype();
941 beaconVehiclePosErr
= bperr
;
943 // Check if the range beacon data can be used to align the vehicle position
944 if ((imuSampleTime_ms
- rngBcnLast3DmeasTime_ms
< 250) && (beaconVehiclePosErr
< 1.0f
) && rngBcnAlignmentCompleted
) {
945 // check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
946 const ftype posDiffSq
= sq(receiverPos
.x
- beaconVehiclePosNED
.x
) + sq(receiverPos
.y
- beaconVehiclePosNED
.y
);
947 const ftype posDiffVar
= sq(beaconVehiclePosErr
) + receiverPosCov
[0][0] + receiverPosCov
[1][1];
948 if (posDiffSq
< 9.0f
* posDiffVar
) {
949 rngBcnGoodToAlign
= true;
950 // Set the EKF origin and magnetic field declination if not previously set
951 if (!validOrigin
&& (PV_AidingMode
!= AID_ABSOLUTE
)) {
952 // get origin from beacon system
954 if (beacon
->get_origin(origin_loc
)) {
955 setOriginLLH(origin_loc
);
957 // set the NE earth magnetic field states using the published declination
958 // and set the corresponding variances and covariances
959 alignMagStateDeclination();
961 // Set the uncertainty of the origin height
962 ekfOriginHgtVar
= sq(beaconVehiclePosErr
);
966 rngBcnGoodToAlign
= false;
969 rngBcnGoodToAlign
= false;
972 // Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
973 rngBcnDataToFuse
= storedRangeBeacon
.recall(rngBcnDataDelayed
, imuDataDelayed
.time_ms
);
975 // Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin
976 if (rngBcnDataToFuse
) {
977 rngBcnDataDelayed
.beacon_posNED
.x
+= bcnPosOffsetNED
.x
;
978 rngBcnDataDelayed
.beacon_posNED
.y
+= bcnPosOffsetNED
.y
;
983 /********************************************************
984 * Independant yaw sensor measurements *
985 ********************************************************/
987 void NavEKF3_core::writeEulerYawAngle(float yawAngle
, float yawAngleErr
, uint32_t timeStamp_ms
, uint8_t type
)
989 // limit update rate to maximum allowed by sensor buffers and fusion process
990 // don't try to write to buffer until the filter has been initialised
991 if (((timeStamp_ms
- yawMeasTime_ms
) < frontend
->sensorIntervalMin_ms
) || !statesInitialised
) {
995 yawAngDataNew
.yawAng
= yawAngle
;
996 yawAngDataNew
.yawAngErr
= yawAngleErr
;
998 yawAngDataNew
.order
= rotationOrder::TAIT_BRYAN_321
;
999 } else if (type
== 1) {
1000 yawAngDataNew
.order
= rotationOrder::TAIT_BRYAN_312
;
1004 yawAngDataNew
.time_ms
= timeStamp_ms
;
1006 storedYawAng
.push(yawAngDataNew
);
1008 yawMeasTime_ms
= timeStamp_ms
;
1011 // 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.
1012 void NavEKF3_core::writeDefaultAirSpeed(float airspeed
, float uncertainty
)
1014 defaultAirSpeed
= airspeed
;
1015 defaultAirSpeedVariance
= sq(uncertainty
);
1018 /********************************************************
1019 * External Navigation Measurements *
1020 ********************************************************/
1022 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
)
1024 #if EK3_FEATURE_EXTERNAL_NAV
1025 // protect against NaN
1026 if (pos
.is_nan() || isnan(posErr
)) {
1030 // limit update rate to maximum allowed by sensor buffers and fusion process
1031 // don't try to write to buffer until the filter has been initialised
1032 if (((timeStamp_ms
- extNavMeasTime_ms
) < frontend
->extNavIntervalMin_ms
) || !statesInitialised
) {
1035 extNavMeasTime_ms
= timeStamp_ms
;
1038 ext_nav_elements extNavDataNew
{};
1040 if (resetTime_ms
!= extNavLastPosResetTime_ms
) {
1041 extNavDataNew
.posReset
= true;
1042 extNavLastPosResetTime_ms
= resetTime_ms
;
1044 extNavDataNew
.posReset
= false;
1047 extNavDataNew
.pos
= pos
.toftype();
1048 extNavDataNew
.posErr
= posErr
;
1050 // calculate timestamp
1051 timeStamp_ms
= timeStamp_ms
- delay_ms
;
1052 // Correct for the average intersampling delay due to the filter update rate
1053 timeStamp_ms
-= localFilterTimeStep_ms
/2;
1054 // Prevent time delay exceeding age of oldest IMU data in the buffer
1055 timeStamp_ms
= MAX(timeStamp_ms
, imuDataDelayed
.time_ms
);
1056 extNavDataNew
.time_ms
= timeStamp_ms
;
1058 // store position data to buffer
1059 storedExtNav
.push(extNavDataNew
);
1061 // protect against attitude or angle being NaN
1062 if (!quat
.is_nan() && !isnan(angErr
)) {
1063 // extract yaw from the attitude
1064 ftype roll_rad
, pitch_rad
, yaw_rad
;
1065 quat
.to_euler(roll_rad
, pitch_rad
, yaw_rad
);
1066 yaw_elements extNavYawAngDataNew
;
1067 extNavYawAngDataNew
.yawAng
= yaw_rad
;
1068 extNavYawAngDataNew
.yawAngErr
= MAX(angErr
, radians(5.0f
)); // ensure yaw accuracy is no better than 5 degrees (some callers may send zero)
1069 extNavYawAngDataNew
.order
= rotationOrder::TAIT_BRYAN_321
; // Euler rotation order is 321 (ZYX)
1070 extNavYawAngDataNew
.time_ms
= timeStamp_ms
;
1071 storedExtNavYawAng
.push(extNavYawAngDataNew
);
1073 #endif // EK3_FEATURE_EXTERNAL_NAV
1076 void NavEKF3_core::writeExtNavVelData(const Vector3f
&vel
, float err
, uint32_t timeStamp_ms
, uint16_t delay_ms
)
1078 #if EK3_FEATURE_EXTERNAL_NAV
1079 // sanity check for NaNs
1080 if (vel
.is_nan() || isnan(err
)) {
1084 if ((timeStamp_ms
- extNavVelMeasTime_ms
) < frontend
->extNavIntervalMin_ms
) {
1088 extNavVelMeasTime_ms
= timeStamp_ms
;
1089 useExtNavVel
= true;
1090 // calculate timestamp
1091 timeStamp_ms
= timeStamp_ms
- delay_ms
;
1092 // Correct for the average intersampling delay due to the filter updaterate
1093 timeStamp_ms
-= localFilterTimeStep_ms
/2;
1094 // Prevent time delay exceeding age of oldest IMU data in the buffer
1095 timeStamp_ms
= MAX(timeStamp_ms
,imuDataDelayed
.time_ms
);
1097 ext_nav_vel_elements extNavVelNew
;
1098 extNavVelNew
.time_ms
= timeStamp_ms
;
1099 extNavVelNew
.vel
= vel
.toftype();
1100 extNavVelNew
.err
= err
;
1101 extNavVelNew
.corrected
= false;
1103 storedExtNavVel
.push(extNavVelNew
);
1104 #endif // EK3_FEATURE_EXTERNAL_NAV
1108 update the GPS selection
1110 void NavEKF3_core::update_gps_selection(void)
1112 const auto &gps
= dal
.gps();
1114 // in normal operation use the primary GPS
1115 selected_gps
= gps
.primary_sensor();
1116 preferred_gps
= selected_gps
;
1118 if (frontend
->_affinity
& EKF_AFFINITY_GPS
) {
1119 if (core_index
< gps
.num_sensors() ) {
1120 // always prefer our core_index, unless we don't have that
1121 // many GPS sensors available
1122 preferred_gps
= core_index
;
1124 if (gps
.status(preferred_gps
) >= AP_DAL_GPS::GPS_OK_FIX_3D
) {
1125 // select our preferred_gps if it has a 3D fix, otherwise
1126 // use the primary GPS
1127 selected_gps
= preferred_gps
;
1133 update the mag selection
1135 void NavEKF3_core::update_mag_selection(void)
1137 const auto &compass
= dal
.compass();
1139 if (frontend
->_affinity
& EKF_AFFINITY_MAG
) {
1140 if (core_index
< compass
.get_count() &&
1141 compass
.healthy(core_index
) &&
1142 compass
.use_for_yaw(core_index
)) {
1143 // use core_index compass if it is healthy
1144 magSelectIndex
= core_index
;
1150 update the baro selection
1152 void NavEKF3_core::update_baro_selection(void)
1154 auto &baro
= dal
.baro();
1156 // in normal operation use the primary baro
1157 selected_baro
= baro
.get_primary();
1159 if (frontend
->_affinity
& EKF_AFFINITY_BARO
) {
1160 if (core_index
< baro
.num_instances() &&
1161 baro
.healthy(core_index
)) {
1162 // use core_index baro if it is healthy
1163 selected_baro
= core_index
;
1169 update the airspeed selection
1171 void NavEKF3_core::update_airspeed_selection(void)
1173 const auto *arsp
= dal
.airspeed();
1174 if (arsp
== nullptr) {
1178 // in normal operation use the primary airspeed sensor
1179 selected_airspeed
= arsp
->get_primary();
1181 if (frontend
->_affinity
& EKF_AFFINITY_ARSP
) {
1182 if (core_index
< arsp
->get_num_sensors() &&
1183 arsp
->healthy(core_index
) &&
1184 arsp
->use(core_index
)) {
1185 // use core_index airspeed if it is healthy
1186 selected_airspeed
= core_index
;
1192 update sensor selections
1194 void NavEKF3_core::update_sensor_selection(void)
1196 update_gps_selection();
1197 update_mag_selection();
1198 update_baro_selection();
1199 update_airspeed_selection();
1203 update timing statistics structure
1205 void NavEKF3_core::updateTimingStatistics(void)
1207 if (timing
.count
== 0) {
1208 timing
.dtIMUavg_max
= dtIMUavg
;
1209 timing
.dtIMUavg_min
= dtIMUavg
;
1210 timing
.dtEKFavg_max
= dtEkfAvg
;
1211 timing
.dtEKFavg_min
= dtEkfAvg
;
1212 timing
.delAngDT_max
= imuDataDelayed
.delAngDT
;
1213 timing
.delAngDT_min
= imuDataDelayed
.delAngDT
;
1214 timing
.delVelDT_max
= imuDataDelayed
.delVelDT
;
1215 timing
.delVelDT_min
= imuDataDelayed
.delVelDT
;
1217 timing
.dtIMUavg_max
= MAX(timing
.dtIMUavg_max
, dtIMUavg
);
1218 timing
.dtIMUavg_min
= MIN(timing
.dtIMUavg_min
, dtIMUavg
);
1219 timing
.dtEKFavg_max
= MAX(timing
.dtEKFavg_max
, dtEkfAvg
);
1220 timing
.dtEKFavg_min
= MIN(timing
.dtEKFavg_min
, dtEkfAvg
);
1221 timing
.delAngDT_max
= MAX(timing
.delAngDT_max
, imuDataDelayed
.delAngDT
);
1222 timing
.delAngDT_min
= MIN(timing
.delAngDT_min
, imuDataDelayed
.delAngDT
);
1223 timing
.delVelDT_max
= MAX(timing
.delVelDT_max
, imuDataDelayed
.delVelDT
);
1224 timing
.delVelDT_min
= MIN(timing
.delVelDT_min
, imuDataDelayed
.delVelDT
);
1230 update estimates of inactive bias states. This keeps inactive IMUs
1231 as hot-spares so we can switch to them without causing a jump in the
1234 void NavEKF3_core::learnInactiveBiases(void)
1236 #if INS_MAX_INSTANCES == 1
1237 inactiveBias
[0].gyro_bias
= stateStruct
.gyro_bias
;
1238 inactiveBias
[0].accel_bias
= stateStruct
.accel_bias
;
1240 const auto &ins
= dal
.ins();
1242 // learn gyro biases
1243 for (uint8_t i
=0; i
<INS_MAX_INSTANCES
; i
++) {
1244 if (!ins
.use_gyro(i
)) {
1245 // can't use this gyro
1248 if (gyro_index_active
== i
) {
1249 // use current estimates from main filter of gyro bias
1250 inactiveBias
[i
].gyro_bias
= stateStruct
.gyro_bias
;
1252 // get filtered gyro and use the difference between the
1253 // corrected gyro on the active IMU and the inactive IMU
1254 // to move the inactive bias towards the right value
1255 Vector3F filtered_gyro_active
= ins
.get_gyro(gyro_index_active
).toftype() - (stateStruct
.gyro_bias
/dtEkfAvg
);
1256 Vector3F filtered_gyro_inactive
= ins
.get_gyro(i
).toftype() - (inactiveBias
[i
].gyro_bias
/dtEkfAvg
);
1257 Vector3F error
= filtered_gyro_active
- filtered_gyro_inactive
;
1259 // prevent a single large error from contaminating bias estimate
1260 const ftype bias_limit
= radians(5);
1261 error
.x
= constrain_ftype(error
.x
, -bias_limit
, bias_limit
);
1262 error
.y
= constrain_ftype(error
.y
, -bias_limit
, bias_limit
);
1263 error
.z
= constrain_ftype(error
.z
, -bias_limit
, bias_limit
);
1265 // slowly bring the inactive gyro in line with the active gyro. This corrects a 5 deg/sec
1266 // gyro bias error in around 1 minute
1267 inactiveBias
[i
].gyro_bias
-= error
* (1.0e-4f
* dtEkfAvg
);
1271 // learn accel biases
1272 for (uint8_t i
=0; i
<INS_MAX_INSTANCES
; i
++) {
1273 if (!ins
.use_accel(i
)) {
1274 // can't use this accel
1277 if (accel_index_active
== i
) {
1278 // use current estimates from main filter of accel bias
1279 inactiveBias
[i
].accel_bias
= stateStruct
.accel_bias
;
1281 // get filtered accel and use the difference between the
1282 // corrected accel on the active IMU and the inactive IMU
1283 // to move the inactive bias towards the right value
1284 Vector3F filtered_accel_active
= ins
.get_accel(accel_index_active
).toftype() - (stateStruct
.accel_bias
/dtEkfAvg
);
1285 Vector3F filtered_accel_inactive
= ins
.get_accel(i
).toftype() - (inactiveBias
[i
].accel_bias
/dtEkfAvg
);
1286 Vector3F error
= filtered_accel_active
- filtered_accel_inactive
;
1288 // prevent a single large error from contaminating bias estimate
1289 const ftype bias_limit
= 1.0; // m/s/s
1290 error
.x
= constrain_ftype(error
.x
, -bias_limit
, bias_limit
);
1291 error
.y
= constrain_ftype(error
.y
, -bias_limit
, bias_limit
);
1292 error
.z
= constrain_ftype(error
.z
, -bias_limit
, bias_limit
);
1294 // slowly bring the inactive accel in line with the active
1295 // accel. This corrects a 0.5 m/s/s accel bias error in
1297 inactiveBias
[i
].accel_bias
-= error
* (1.0e-4f
* dtEkfAvg
);
1304 return declination in radians
1306 ftype
NavEKF3_core::MagDeclination(void) const
1308 // if we are using the WMM tables then use the table declination
1309 // to ensure consistency with the table mag field. Otherwise use
1310 // the declination from the compass library
1311 if (have_table_earth_field
&& frontend
->_mag_ef_limit
> 0) {
1312 return table_declination
;
1314 if (!use_compass()) {
1317 return dal
.compass().get_declination();
1321 Update the on ground and not moving check.
1322 Should be called once per IMU update.
1323 Only updates when on ground and when operating without a magnetometer
1325 void NavEKF3_core::updateMovementCheck(void)
1327 const AP_NavEKF_Source::SourceYaw yaw_source
= frontend
->sources
.getYawSource();
1328 const bool runCheck
= onGround
&& (yaw_source
== AP_NavEKF_Source::SourceYaw::GPS
|| yaw_source
== AP_NavEKF_Source::SourceYaw::GPS_COMPASS_FALLBACK
||
1329 yaw_source
== AP_NavEKF_Source::SourceYaw::EXTNAV
|| yaw_source
== AP_NavEKF_Source::SourceYaw::GSF
|| !use_compass());
1332 onGroundNotMoving
= false;
1336 const ftype gyro_limit
= radians(3.0f
);
1337 const ftype gyro_diff_limit
= 0.2f
;
1338 const ftype accel_limit
= 1.0f
;
1339 const ftype accel_diff_limit
= 5.0f
;
1340 const ftype hysteresis_ratio
= 0.7f
;
1341 const ftype dtEkfAvgInv
= 1.0f
/ dtEkfAvg
;
1343 // get latest bias corrected gyro and accelerometer data
1344 const auto &ins
= dal
.ins();
1345 Vector3F gyro
= ins
.get_gyro(gyro_index_active
).toftype() - stateStruct
.gyro_bias
* dtEkfAvgInv
;
1346 Vector3F accel
= ins
.get_accel(accel_index_active
).toftype() - stateStruct
.accel_bias
* dtEkfAvgInv
;
1348 if (!prevOnGround
) {
1351 onGroundNotMoving
= false;
1352 gyro_diff
= gyro_diff_limit
;
1353 accel_diff
= accel_diff_limit
;
1357 // calculate a gyro rate of change metric
1358 Vector3F temp
= (gyro
- gyro_prev
) * dtEkfAvgInv
;
1360 gyro_diff
= 0.99f
* gyro_diff
+ 0.01f
* temp
.length();
1362 // calculate a acceleration rate of change metric
1363 temp
= (accel
- accel_prev
) * dtEkfAvgInv
;
1365 accel_diff
= 0.99f
* accel_diff
+ 0.01f
* temp
.length();
1367 const ftype gyro_length_ratio
= gyro
.length() / gyro_limit
;
1368 const ftype accel_length_ratio
= (accel
.length() - GRAVITY_MSS
) / accel_limit
;
1369 const ftype gyro_diff_ratio
= gyro_diff
/ gyro_diff_limit
;
1370 const ftype accel_diff_ratio
= accel_diff
/ accel_diff_limit
;
1371 bool logStatusChange
= false;
1372 if (onGroundNotMoving
) {
1373 if (gyro_length_ratio
> frontend
->_ognmTestScaleFactor
||
1374 fabsF(accel_length_ratio
) > frontend
->_ognmTestScaleFactor
||
1375 gyro_diff_ratio
> frontend
->_ognmTestScaleFactor
||
1376 accel_diff_ratio
> frontend
->_ognmTestScaleFactor
)
1378 onGroundNotMoving
= false;
1379 logStatusChange
= true;
1381 } else if (gyro_length_ratio
< frontend
->_ognmTestScaleFactor
* hysteresis_ratio
&&
1382 fabsF(accel_length_ratio
) < frontend
->_ognmTestScaleFactor
* hysteresis_ratio
&&
1383 gyro_diff_ratio
< frontend
->_ognmTestScaleFactor
* hysteresis_ratio
&&
1384 accel_diff_ratio
< frontend
->_ognmTestScaleFactor
* hysteresis_ratio
)
1386 onGroundNotMoving
= true;
1387 logStatusChange
= true;
1390 if (logStatusChange
|| imuSampleTime_ms
- lastMoveCheckLogTime_ms
> 200) {
1391 lastMoveCheckLogTime_ms
= imuSampleTime_ms
;
1392 const struct log_XKFM pkt
{
1393 LOG_PACKET_HEADER_INIT(LOG_XKFM_MSG
),
1394 time_us
: dal
.micros64(),
1396 ongroundnotmoving
: onGroundNotMoving
,
1397 gyro_length_ratio
: float(gyro_length_ratio
),
1398 accel_length_ratio
: float(accel_length_ratio
),
1399 gyro_diff_ratio
: float(gyro_diff_ratio
),
1400 accel_diff_ratio
: float(accel_diff_ratio
),
1402 AP::logger().WriteBlock(&pkt
, sizeof(pkt
));
1406 void NavEKF3_core::SampleDragData(const imu_elements
&imu
)
1408 #if EK3_FEATURE_DRAG_FUSION
1409 // Average and down sample to 5Hz
1410 const ftype bcoef_x
= frontend
->_ballisticCoef_x
;
1411 const ftype bcoef_y
= frontend
->_ballisticCoef_y
;
1412 const ftype mcoef
= frontend
->_momentumDragCoef
.get();
1413 const bool using_bcoef_x
= bcoef_x
> 1.0f
;
1414 const bool using_bcoef_y
= bcoef_y
> 1.0f
;
1415 const bool using_mcoef
= mcoef
> 0.001f
;
1416 if (!using_bcoef_x
&& !using_bcoef_y
&& !using_mcoef
) {
1418 dragFusionEnabled
= false;
1422 dragFusionEnabled
= true;
1424 // down-sample the drag specific force data by accumulating and calculating the mean when
1425 // sufficient samples have been collected
1429 // note acceleration is accumulated as a delta velocity
1430 dragDownSampled
.accelXY
.x
+= imu
.delVel
.x
;
1431 dragDownSampled
.accelXY
.y
+= imu
.delVel
.y
;
1432 dragDownSampled
.time_ms
+= imu
.time_ms
;
1433 dragSampleTimeDelta
+= imu
.delVelDT
;
1435 // calculate and store means from accumulated values
1436 if (dragSampleTimeDelta
> 0.2f
- 0.5f
* EKF_TARGET_DT
) {
1437 // note conversion from accumulated delta velocity to acceleration
1438 dragDownSampled
.accelXY
.x
/= dragSampleTimeDelta
;
1439 dragDownSampled
.accelXY
.y
/= dragSampleTimeDelta
;
1440 dragDownSampled
.time_ms
/= dragSampleCount
;
1443 storedDrag
.push(dragDownSampled
);
1445 // reset accumulators
1446 dragSampleCount
= 0;
1447 dragDownSampled
.accelXY
.zero();
1448 dragDownSampled
.time_ms
= 0;
1449 dragSampleTimeDelta
= 0.0f
;
1451 #endif // EK3_FEATURE_DRAG_FUSION
1455 get the earth mag field
1457 void NavEKF3_core::getEarthFieldTable(const Location
&loc
)
1459 table_earth_field_ga
= AP_Declination::get_earth_field_ga(loc
).toftype();
1460 table_declination
= radians(AP_Declination::get_declination(loc
.lat
*1.0e-7,
1462 have_table_earth_field
= true;
1466 update earth field, called at 1Hz
1468 void NavEKF3_core::checkUpdateEarthField(void)
1470 if (have_table_earth_field
&& filterStatus
.flags
.using_gps
) {
1471 Location loc
= EKF_origin
;
1472 loc
.offset(stateStruct
.position
.x
, stateStruct
.position
.y
);
1473 getEarthFieldTable(loc
);