2015-10-06 18:20:43 -03:00
# include <AP_HAL/AP_HAL.h>
# include "AP_NavEKF2_core.h"
2016-07-18 23:16:25 -03:00
# include <GCS_MAVLink/GCS.h>
2020-11-05 19:30:16 -04:00
# include <AP_DAL/AP_DAL.h>
2022-03-04 02:13:20 -04:00
# include <AP_InternalError/AP_InternalError.h>
2015-10-06 18:20:43 -03:00
2023-11-07 18:23:40 -04:00
# if AP_RANGEFINDER_ENABLED
2015-10-06 18:20:43 -03:00
extern const AP_HAL : : HAL & hal ;
/********************************************************
* OPT FLOW AND RANGE FINDER *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// Read the range finder and take new measurements if available
2015-11-12 04:07:52 -04:00
// Apply a median filter
2015-10-06 18:20:43 -03:00
void NavEKF2_core : : readRangeFinder ( void )
{
uint8_t midIndex ;
uint8_t maxIndex ;
uint8_t minIndex ;
2017-02-09 06:27:32 -04:00
2015-10-06 18:20:43 -03:00
// get theoretical correct range when the vehicle is on the ground
2017-02-09 06:27:32 -04:00
// don't allow range to go below 5cm because this can cause problems with optical flow processing
2020-11-07 02:24:13 -04:00
const auto * _rng = dal . rangefinder ( ) ;
2019-11-26 02:43:24 -04:00
if ( _rng = = nullptr ) {
return ;
}
rngOnGnd = MAX ( _rng - > ground_clearance_cm_orient ( ROTATION_PITCH_270 ) * 0.01f , 0.05f ) ;
2015-11-12 04:07:52 -04:00
// read range finder at 20Hz
// TODO better way of knowing if it has new data
2019-11-26 02:43:24 -04:00
if ( _rng & & ( imuSampleTime_ms - lastRngMeasTime_ms ) > 50 ) {
2015-11-12 04:07:52 -04:00
// reset the timer used to control the measurement rate
lastRngMeasTime_ms = imuSampleTime_ms ;
// store samples and sample time into a ring buffer if valid
2016-07-12 05:56:58 -03:00
// use data from two range finders if available
2015-11-12 04:07:52 -04:00
2022-06-05 23:25:45 -03:00
for ( uint8_t sensorIndex = 0 ; sensorIndex < ARRAY_SIZE ( rngMeasIndex ) ; sensorIndex + + ) {
2020-11-05 19:30:16 -04:00
auto * sensor = _rng - > get_backend ( sensorIndex ) ;
2017-08-08 03:06:48 -03:00
if ( sensor = = nullptr ) {
continue ;
}
2020-11-05 19:30:16 -04:00
if ( ( sensor - > orientation ( ) = = ROTATION_PITCH_270 ) & & ( sensor - > status ( ) = = AP_DAL_RangeFinder : : Status : : Good ) ) {
2016-07-12 05:56:58 -03:00
rngMeasIndex [ sensorIndex ] + + ;
if ( rngMeasIndex [ sensorIndex ] > 2 ) {
rngMeasIndex [ sensorIndex ] = 0 ;
}
storedRngMeasTime_ms [ sensorIndex ] [ rngMeasIndex [ sensorIndex ] ] = imuSampleTime_ms - 25 ;
2017-08-08 03:06:48 -03:00
storedRngMeas [ sensorIndex ] [ rngMeasIndex [ sensorIndex ] ] = sensor - > distance_cm ( ) * 0.01f ;
2020-05-20 07:23:05 -03:00
} else {
continue ;
2016-07-12 05:56:58 -03:00
}
2015-11-12 04:07:52 -04:00
2016-07-12 05:56:58 -03:00
// check for three fresh samples
2022-06-05 23:25:45 -03:00
bool sampleFresh [ DOWNWARD_RANGEFINDER_MAX_INSTANCES ] [ 3 ] = { } ;
2016-07-12 05:56:58 -03:00
for ( uint8_t index = 0 ; index < = 2 ; index + + ) {
sampleFresh [ sensorIndex ] [ index ] = ( imuSampleTime_ms - storedRngMeasTime_ms [ sensorIndex ] [ index ] ) < 500 ;
2015-10-06 18:20:43 -03:00
}
2016-07-12 05:56:58 -03:00
// find the median value if we have three fresh samples
if ( sampleFresh [ sensorIndex ] [ 0 ] & & sampleFresh [ sensorIndex ] [ 1 ] & & sampleFresh [ sensorIndex ] [ 2 ] ) {
if ( storedRngMeas [ sensorIndex ] [ 0 ] > storedRngMeas [ sensorIndex ] [ 1 ] ) {
minIndex = 1 ;
maxIndex = 0 ;
} else {
2016-10-19 07:32:38 -03:00
minIndex = 0 ;
maxIndex = 1 ;
2016-07-12 05:56:58 -03:00
}
if ( storedRngMeas [ sensorIndex ] [ 2 ] > storedRngMeas [ sensorIndex ] [ maxIndex ] ) {
midIndex = maxIndex ;
} else if ( storedRngMeas [ sensorIndex ] [ 2 ] < storedRngMeas [ sensorIndex ] [ minIndex ] ) {
midIndex = minIndex ;
} else {
midIndex = 2 ;
}
// don't allow time to go backwards
if ( storedRngMeasTime_ms [ sensorIndex ] [ midIndex ] > rangeDataNew . time_ms ) {
rangeDataNew . time_ms = storedRngMeasTime_ms [ sensorIndex ] [ midIndex ] ;
}
// limit the measured range to be no less than the on-ground range
rangeDataNew . rng = MAX ( storedRngMeas [ sensorIndex ] [ midIndex ] , rngOnGnd ) ;
2016-10-11 18:27:43 -03:00
// get position in body frame for the current sensor
2016-10-27 01:47:26 -03:00
rangeDataNew . sensor_idx = sensorIndex ;
2016-10-11 18:27:43 -03:00
2016-07-12 05:56:58 -03:00
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
storedRange . push ( rangeDataNew ) ;
2016-08-19 22:31:09 -03:00
// indicate we have updated the measurement
rngValidMeaTime_ms = imuSampleTime_ms ;
2016-07-12 05:56:58 -03:00
} else if ( ! takeOffDetected & & ( ( imuSampleTime_ms - rngValidMeaTime_ms ) > 200 ) ) {
// before takeoff we assume on-ground range value if there is no data
rangeDataNew . time_ms = imuSampleTime_ms ;
rangeDataNew . rng = rngOnGnd ;
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
storedRange . push ( rangeDataNew ) ;
2016-08-19 22:31:09 -03:00
// indicate we have updated the measurement
rngValidMeaTime_ms = imuSampleTime_ms ;
2016-07-12 05:56:58 -03:00
2016-08-19 22:31:09 -03:00
}
2015-10-06 18:20:43 -03:00
}
}
}
2023-11-07 18:23:40 -04:00
# endif
2015-10-06 18:20:43 -03:00
// write the raw optical flow measurements
// this needs to be called externally.
2022-10-24 00:01:04 -03:00
void NavEKF2_core : : writeOptFlowMeas ( const uint8_t rawFlowQuality , const Vector2f & rawFlowRates , const Vector2f & rawGyroRates , const uint32_t msecFlowMeas , const Vector3f & posOffset , float heightOverride )
2015-10-06 18:20:43 -03:00
{
// The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
// The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
// A positive X rate is produced by a positive sensor rotation about the X axis
// A positive Y rate is produced by a positive sensor rotation about the Y axis
// This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
// 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
flowMeaTime_ms = imuSampleTime_ms ;
// calculate bias errors on flow sensor gyro rates, but protect against spikes in data
// reset the accumulated body delta angle and time
// don't do the calculation if not enough time lapsed for a reliable body rate measurement
if ( delTimeOF > 0.01f ) {
2021-05-04 08:12:23 -03:00
flowGyroBias . x = 0.99f * flowGyroBias . x + 0.01f * constrain_ftype ( ( rawGyroRates . x - delAngBodyOF . x / delTimeOF ) , - 0.1f , 0.1f ) ;
flowGyroBias . y = 0.99f * flowGyroBias . y + 0.01f * constrain_ftype ( ( rawGyroRates . y - delAngBodyOF . y / delTimeOF ) , - 0.1f , 0.1f ) ;
2015-10-06 18:20:43 -03:00
delAngBodyOF . zero ( ) ;
delTimeOF = 0.0f ;
}
2016-10-07 06:35:55 -03:00
// by definition if this function is called, then flow measurements have been provided so we
// need to run the optical flow takeoff detection
detectOptFlowTakeoff ( ) ;
2015-10-06 18:20:43 -03:00
// calculate rotation matrices at mid sample time for flow observations
stateStruct . quat . rotation_matrix ( Tbn_flow ) ;
// don't use data with a low quality indicator or extreme rates (helps catch corrupt sensor data)
if ( ( rawFlowQuality > 0 ) & & rawFlowRates . length ( ) < 4.2f & & rawGyroRates . length ( ) < 4.2f ) {
2016-10-11 20:45:53 -03:00
// correct flow sensor body rates for bias and write
ofDataNew . bodyRadXYZ . x = rawGyroRates . x - flowGyroBias . x ;
ofDataNew . bodyRadXYZ . y = rawGyroRates . y - flowGyroBias . y ;
// the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
if ( delTimeOF > 0.001f ) {
// first preference is to use the rate averaged over the same sampling period as the flow sensor
ofDataNew . bodyRadXYZ . z = delAngBodyOF . z / delTimeOF ;
} else if ( imuDataNew . delAngDT > 0.001f ) {
// second preference is to use most recent IMU data
ofDataNew . bodyRadXYZ . z = imuDataNew . delAng . z / imuDataNew . delAngDT ;
} else {
// third preference is use zero
ofDataNew . bodyRadXYZ . z = 0.0f ;
}
// write uncorrected flow rate measurements
2015-10-06 18:20:43 -03:00
// note correction for different axis and sign conventions used by the px4flow sensor
2021-05-04 08:12:23 -03:00
ofDataNew . flowRadXY = ( - rawFlowRates ) . toftype ( ) ; // raw (non motion compensated) optical flow angular rate about the X axis (rad/sec)
2016-10-11 18:28:58 -03:00
// write the flow sensor position in body frame
2021-05-04 08:12:23 -03:00
ofDataNew . body_offset = posOffset . toftype ( ) ;
2022-10-24 00:01:04 -03:00
// write the flow sensor height override
ofDataNew . heightOverride = heightOverride ;
2015-10-06 18:20:43 -03:00
// write flow rate measurements corrected for body rates
2016-10-11 20:45:53 -03:00
ofDataNew . flowRadXYcomp . x = ofDataNew . flowRadXY . x + ofDataNew . bodyRadXYZ . x ;
ofDataNew . flowRadXYcomp . y = ofDataNew . flowRadXY . y + ofDataNew . bodyRadXYZ . y ;
2015-10-06 18:20:43 -03:00
// record time last observation was received so we can detect loss of data elsewhere
flowValidMeaTime_ms = imuSampleTime_ms ;
// estimate sample time of the measurement
2015-11-04 21:00:57 -04:00
ofDataNew . time_ms = imuSampleTime_ms - frontend - > _flowDelay_ms - frontend - > flowTimeDeltaAvg_ms / 2 ;
2015-11-09 20:25:44 -04:00
// Correct for the average intersampling delay due to the filter updaterate
ofDataNew . time_ms - = localFilterTimeStep_ms / 2 ;
2015-10-18 19:11:58 -03:00
// Prevent time delay exceeding age of oldest IMU data in the buffer
2015-11-27 13:11:58 -04:00
ofDataNew . time_ms = MAX ( ofDataNew . time_ms , imuDataDelayed . time_ms ) ;
2015-10-06 18:20:43 -03:00
// Save data to buffer
2015-11-20 18:05:12 -04:00
storedOF . push ( ofDataNew ) ;
2015-10-06 18:20:43 -03:00
// Check for data at the fusion time horizon
2015-11-13 18:28:45 -04:00
flowDataToFuse = storedOF . recall ( ofDataDelayed , imuDataDelayed . time_ms ) ;
2015-10-06 18:20:43 -03:00
}
}
/********************************************************
* MAGNETOMETER *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2020-08-27 23:42:42 -03:00
// try changing to another compass
void NavEKF2_core : : tryChangeCompass ( void )
{
2020-11-07 02:24:13 -04:00
const auto & compass = dal . compass ( ) ;
2020-08-27 23:42:42 -03:00
const uint8_t maxCount = compass . get_count ( ) ;
// search through the list of magnetometers
for ( uint8_t i = 1 ; i < maxCount ; i + + ) {
uint8_t tempIndex = magSelectIndex + i ;
// loop back to the start index if we have exceeded the bounds
if ( tempIndex > = maxCount ) {
tempIndex - = maxCount ;
}
// if the magnetometer is allowed to be used for yaw and has a different index, we start using it
if ( compass . healthy ( tempIndex ) & & compass . use_for_yaw ( tempIndex ) & & tempIndex ! = magSelectIndex ) {
magSelectIndex = tempIndex ;
2020-11-05 19:30:16 -04:00
GCS_SEND_TEXT ( MAV_SEVERITY_INFO , " EKF2 IMU%u switching to compass %u " , ( unsigned ) imu_index , magSelectIndex ) ;
2020-08-27 23:42:42 -03:00
// reset the timeout flag and timer
magTimeout = false ;
lastHealthyMagTime_ms = imuSampleTime_ms ;
// zero the learned magnetometer bias states
stateStruct . body_magfield . zero ( ) ;
// clear the measurement buffer
storedMag . reset ( ) ;
// clear the data waiting flag so that we do not use any data pending from the previous sensor
magDataToFuse = false ;
// request a reset of the magnetic field states
magStateResetRequest = true ;
// declare the field unlearned so that the reset request will be obeyed
magFieldLearned = false ;
2020-09-16 21:39:04 -03:00
// reset body mag variances on next CovariancePrediction
needMagBodyVarReset = true ;
2020-08-27 23:42:42 -03:00
return ;
}
}
}
2015-10-06 18:20:43 -03:00
// check for new magnetometer data and update store measurements if available
void NavEKF2_core : : readMagData ( )
{
2021-07-25 21:32:12 -03:00
const auto & compass = dal . compass ( ) ;
if ( ! compass . available ( ) ) {
2016-05-19 01:16:40 -03:00
allMagSensorsFailed = true ;
return ;
}
2020-03-11 05:02:40 -03:00
2015-11-15 17:05:08 -04:00
// If we are a vehicle with a sideslip constraint to aid yaw estimation and we have timed out on our last avialable
// magnetometer, then declare the magnetometers as failed for this flight
2020-03-11 05:02:40 -03:00
const uint8_t maxCount = compass . get_count ( ) ;
2015-11-15 17:05:08 -04:00
if ( allMagSensorsFailed | | ( magTimeout & & assume_zero_sideslip ( ) & & magSelectIndex > = maxCount - 1 & & inFlight ) ) {
allMagSensorsFailed = true ;
return ;
}
2020-03-11 05:02:40 -03:00
if ( compass . learn_offsets_enabled ( ) ) {
2018-10-19 02:06:55 -03:00
// while learning offsets keep all mag states reset
InitialiseVariablesMag ( ) ;
wasLearningCompass_ms = imuSampleTime_ms ;
} else if ( wasLearningCompass_ms ! = 0 & & imuSampleTime_ms - wasLearningCompass_ms > 1000 ) {
wasLearningCompass_ms = 0 ;
// force a new yaw alignment 1s after learning completes. The
// delay is to ensure any buffered mag samples are discarded
yawAlignComplete = false ;
InitialiseVariablesMag ( ) ;
}
2020-08-28 08:52:38 -03:00
// If the magnetometer has timed out (been rejected for too long), we find another magnetometer to use if available
// 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
// 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
// if the timeout is due to a sensor failure, then declare a timeout regardless of onground status
if ( maxCount > 1 ) {
bool fusionTimeout = magTimeout & & ! onGround & & imuSampleTime_ms - ekfStartTime_ms > 30000 ;
bool sensorTimeout = ! compass . healthy ( magSelectIndex ) & & imuSampleTime_ms - lastMagRead_ms > frontend - > magFailTimeLimit_ms ;
if ( fusionTimeout | | sensorTimeout ) {
tryChangeCompass ( ) ;
}
}
2015-10-20 23:46:36 -03:00
// do not accept new compass data faster than 14Hz (nominal rate is 10Hz) to prevent high processor loading
// because magnetometer fusion is an expensive step and we could overflow the FIFO buffer
2020-08-27 23:42:42 -03:00
if ( use_compass ( ) & &
compass . healthy ( magSelectIndex ) & &
compass . last_update_usec ( magSelectIndex ) - lastMagUpdate_us > 70000 ) {
2015-11-08 05:41:08 -04:00
2016-03-17 19:58:54 -03:00
// detect changes to magnetometer offset parameters and reset states
2021-05-04 08:12:23 -03:00
Vector3F nowMagOffsets = compass . get_offsets ( magSelectIndex ) . toftype ( ) ;
2016-04-20 20:56:22 -03:00
bool changeDetected = lastMagOffsetsValid & & ( nowMagOffsets ! = lastMagOffsets ) ;
2016-03-17 19:58:54 -03:00
if ( changeDetected ) {
// zero the learned magnetometer bias states
stateStruct . body_magfield . zero ( ) ;
// clear the measurement buffer
storedMag . reset ( ) ;
2020-09-16 21:39:04 -03:00
// reset body mag variances on next
// CovariancePrediction. This copes with possible errors
// in the new offsets
needMagBodyVarReset = true ;
2016-03-17 19:58:54 -03:00
}
lastMagOffsets = nowMagOffsets ;
lastMagOffsetsValid = true ;
2015-10-06 18:20:43 -03:00
// store time of last measurement update
2020-03-11 05:02:40 -03:00
lastMagUpdate_us = compass . last_update_usec ( magSelectIndex ) ;
2015-10-06 18:20:43 -03:00
2020-08-27 23:42:42 -03:00
// Magnetometer data at the current time horizon
mag_elements magDataNew ;
2015-10-06 18:20:43 -03:00
// estimate of time magnetometer measurement was taken, allowing for delays
2015-11-04 21:00:57 -04:00
magDataNew . time_ms = imuSampleTime_ms - frontend - > magDelay_ms ;
2015-10-06 18:20:43 -03:00
2015-11-09 20:25:44 -04:00
// Correct for the average intersampling delay due to the filter updaterate
magDataNew . time_ms - = localFilterTimeStep_ms / 2 ;
2015-10-15 09:01:04 -03:00
2015-10-06 18:20:43 -03:00
// read compass data and scale to improve numerical conditioning
2021-05-04 08:12:23 -03:00
magDataNew . mag = compass . get_field ( magSelectIndex ) . toftype ( ) * 0.001f ;
2015-10-06 18:20:43 -03:00
// check for consistent data between magnetometers
2020-03-11 05:02:40 -03:00
consistentMagData = compass . consistent ( ) ;
2015-10-06 18:20:43 -03:00
// save magnetometer measurement to buffer to be fused later
2015-11-20 18:05:12 -04:00
storedMag . push ( magDataNew ) ;
2020-08-27 23:42:42 -03:00
// remember time we read compass, to detect compass sensor failure
lastMagRead_ms = imuSampleTime_ms ;
2015-10-06 18:20:43 -03:00
}
}
/********************************************************
* Inertial Measurements *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2015-11-09 20:25:44 -04:00
/*
* Read IMU delta angle and delta velocity measurements and downsample to 100 Hz
* for storage in the data buffers used by the EKF . If the IMU data arrives at
* lower rate than 100 Hz , then no downsampling or upsampling will be performed .
* Downsampling is done using a method that does not introduce coning or sculling
* errors .
*/
2015-10-06 18:20:43 -03:00
void NavEKF2_core : : readIMUData ( )
{
2020-11-07 02:24:13 -04:00
const auto & ins = dal . ins ( ) ;
2015-10-06 18:20:43 -03:00
// average IMU sampling rate
2015-12-25 23:12:20 -04:00
dtIMUavg = ins . get_loop_delta_t ( ) ;
2015-10-06 18:20:43 -03:00
2015-11-04 22:05:11 -04:00
// use the nominated imu or primary if not available
2019-07-04 04:11:57 -03:00
uint8_t accel_active , gyro_active ;
2015-11-04 22:05:11 -04:00
if ( ins . use_accel ( imu_index ) ) {
2019-07-04 04:11:57 -03:00
accel_active = imu_index ;
2015-10-19 02:46:49 -03:00
} else {
2024-06-25 06:05:26 -03:00
accel_active = ins . get_first_usable_accel ( ) ;
2015-10-19 02:46:49 -03:00
}
2015-10-06 18:20:43 -03:00
2015-11-04 22:05:11 -04:00
if ( ins . use_gyro ( imu_index ) ) {
2019-07-04 04:11:57 -03:00
gyro_active = imu_index ;
2015-11-04 22:05:11 -04:00
} else {
2024-06-25 06:05:26 -03:00
gyro_active = ins . get_first_usable_gyro ( ) ;
2019-07-04 04:11:57 -03:00
}
if ( gyro_active ! = gyro_index_active ) {
// we are switching active gyro at runtime. Copy over the
// biases we have learned from the previously inactive
// gyro. We don't re-init the bias uncertainty as it should
// have the same uncertainty as the previously active gyro
stateStruct . gyro_bias = inactiveBias [ gyro_active ] . gyro_bias ;
gyro_index_active = gyro_active ;
// use the gyro scale factor we have previously used on this
// IMU (if any). We don't reset the variances as we don't want
// errors after switching to be mis-assigned to the gyro scale
// factor
stateStruct . gyro_scale = inactiveBias [ gyro_active ] . gyro_scale ;
2015-11-04 22:05:11 -04:00
}
2015-10-06 18:20:43 -03:00
2019-07-04 04:11:57 -03:00
if ( accel_active ! = accel_index_active ) {
// switch to the learned accel bias for this IMU
stateStruct . accel_zbias = inactiveBias [ accel_active ] . accel_zbias ;
accel_index_active = accel_active ;
}
// update the inactive bias states
learnInactiveBiases ( ) ;
readDeltaVelocity ( accel_index_active , imuDataNew . delVel , imuDataNew . delVelDT ) ;
2021-05-04 08:12:23 -03:00
accelPosOffset = ins . get_imu_pos_offset ( accel_index_active ) . toftype ( ) ;
2019-07-04 04:11:57 -03:00
imuDataNew . accel_index = accel_index_active ;
// Get delta angle data from primary gyro or primary if not available
readDeltaAngle ( gyro_index_active , imuDataNew . delAng , imuDataNew . delAngDT ) ;
imuDataNew . gyro_index = gyro_index_active ;
2015-11-09 20:25:44 -04:00
// Get current time stamp
2015-10-06 18:20:43 -03:00
imuDataNew . time_ms = imuSampleTime_ms ;
2019-07-04 04:11:57 -03:00
// use the most recent IMU index for the downsampled IMU
// data. This isn't strictly correct if we switch IMUs between
// samples
imuDataDownSampledNew . gyro_index = imuDataNew . gyro_index ;
imuDataDownSampledNew . accel_index = imuDataNew . accel_index ;
2015-11-09 20:25:44 -04:00
// Accumulate the measurement time interval for the delta velocity and angle data
imuDataDownSampledNew . delAngDT + = imuDataNew . delAngDT ;
imuDataDownSampledNew . delVelDT + = imuDataNew . delVelDT ;
// Rotate quaternon atitude from previous to new and normalise.
// Accumulation using quaternions prevents introduction of coning errors due to downsampling
2016-05-31 18:26:34 -03:00
imuQuatDownSampleNew . rotate ( imuDataNew . delAng ) ;
2015-11-09 20:25:44 -04:00
imuQuatDownSampleNew . normalize ( ) ;
2016-05-31 20:05:59 -03:00
// Rotate the latest delta velocity into body frame at the start of accumulation
2021-05-04 08:12:23 -03:00
Matrix3F deltaRotMat ;
2016-05-31 18:26:34 -03:00
imuQuatDownSampleNew . rotation_matrix ( deltaRotMat ) ;
2016-05-31 20:05:59 -03:00
// Apply the delta velocity to the delta velocity accumulator
2016-05-31 18:26:34 -03:00
imuDataDownSampledNew . delVel + = deltaRotMat * imuDataNew . delVel ;
2015-11-09 20:25:44 -04:00
// Keep track of the number of IMU frames since the last state prediction
framesSincePredict + + ;
2017-05-01 00:50:52 -03:00
/*
* If the target EKF time step has been accumulated , and the frontend has allowed start of a new predict cycle ,
* then store the accumulated IMU data to be used by the state prediction , ignoring the frontend permission if more
* than twice the target time has lapsed . Adjust the target EKF step time threshold to allow for timing jitter in the
* IMU data .
*/
2017-04-28 21:48:49 -03:00
if ( ( dtIMUavg * ( float ) framesSincePredict > = ( EKF_TARGET_DT - ( dtIMUavg * 0.5 ) ) & &
startPredictEnabled ) | | ( dtIMUavg * ( float ) framesSincePredict > = 2.0f * EKF_TARGET_DT ) ) {
2016-06-29 09:38:45 -03:00
2015-11-09 20:25:44 -04:00
// convert the accumulated quaternion to an equivalent delta angle
imuQuatDownSampleNew . to_axis_angle ( imuDataDownSampledNew . delAng ) ;
2016-06-29 09:38:45 -03:00
2015-11-09 20:25:44 -04:00
// Time stamp the data
imuDataDownSampledNew . time_ms = imuSampleTime_ms ;
2016-06-29 09:38:45 -03:00
2015-11-09 20:25:44 -04:00
// Write data to the FIFO IMU buffer
2015-11-20 18:05:12 -04:00
storedIMU . push_youngest_element ( imuDataDownSampledNew ) ;
2016-06-29 09:38:45 -03:00
2016-07-04 03:19:13 -03:00
// calculate the achieved average time step rate for the EKF
2021-05-04 08:12:23 -03:00
ftype dtNow = constrain_ftype ( 0.5f * ( imuDataDownSampledNew . delAngDT + imuDataDownSampledNew . delVelDT ) , 0.0f , 10.0f * EKF_TARGET_DT ) ;
2016-07-04 03:19:13 -03:00
dtEkfAvg = 0.98f * dtEkfAvg + 0.02f * dtNow ;
2015-11-09 20:25:44 -04:00
// zero the accumulated IMU data and quaternion
imuDataDownSampledNew . delAng . zero ( ) ;
imuDataDownSampledNew . delVel . zero ( ) ;
imuDataDownSampledNew . delAngDT = 0.0f ;
imuDataDownSampledNew . delVelDT = 0.0f ;
2019-07-04 04:11:57 -03:00
imuDataDownSampledNew . gyro_index = gyro_index_active ;
imuDataDownSampledNew . accel_index = accel_index_active ;
2015-11-09 20:25:44 -04:00
imuQuatDownSampleNew [ 0 ] = 1.0f ;
imuQuatDownSampleNew [ 3 ] = imuQuatDownSampleNew [ 2 ] = imuQuatDownSampleNew [ 1 ] = 0.0f ;
2016-06-29 09:38:45 -03:00
2015-11-09 20:25:44 -04:00
// reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle
framesSincePredict = 0 ;
2016-06-29 09:38:45 -03:00
2018-05-07 13:42:11 -03:00
// set the flag to let the filter know it has new IMU data and needs to run
2015-11-09 20:25:44 -04:00
runUpdates = true ;
2016-06-29 09:38:45 -03:00
2016-06-15 07:20:50 -03:00
// extract the oldest available data from the FIFO buffer
2020-11-14 01:11:21 -04:00
imuDataDelayed = storedIMU . get_oldest_element ( ) ;
2016-06-29 09:38:45 -03:00
// protect against delta time going to zero
// TODO - check if calculations can tolerate 0
2021-05-04 08:12:23 -03:00
ftype minDT = 0.1f * dtEkfAvg ;
2016-06-15 07:20:50 -03:00
imuDataDelayed . delAngDT = MAX ( imuDataDelayed . delAngDT , minDT ) ;
imuDataDelayed . delVelDT = MAX ( imuDataDelayed . delVelDT , minDT ) ;
2016-06-29 09:38:45 -03:00
2017-04-27 21:49:58 -03:00
updateTimingStatistics ( ) ;
2016-06-15 07:20:50 -03:00
// correct the extracted IMU data for sensor errors
2016-06-29 09:38:45 -03:00
delAngCorrected = imuDataDelayed . delAng ;
delVelCorrected = imuDataDelayed . delVel ;
2019-07-04 04:11:57 -03:00
correctDeltaAngle ( delAngCorrected , imuDataDelayed . delAngDT , imuDataDelayed . gyro_index ) ;
correctDeltaVelocity ( delVelCorrected , imuDataDelayed . delVelDT , imuDataDelayed . accel_index ) ;
2016-06-29 09:38:45 -03:00
2015-11-09 20:25:44 -04:00
} else {
// we don't have new IMU data in the buffer so don't run filter updates on this time step
runUpdates = false ;
}
2015-10-06 18:20:43 -03:00
}
2015-10-30 03:07:38 -03:00
// read the delta velocity and corresponding time interval from the IMU
// return false if data is not available
2021-05-04 08:12:23 -03:00
bool NavEKF2_core : : readDeltaVelocity ( uint8_t ins_index , Vector3F & dVel , ftype & dVel_dt ) {
2020-11-07 02:24:13 -04:00
const auto & ins = dal . ins ( ) ;
2015-10-06 18:20:43 -03:00
if ( ins_index < ins . get_accel_count ( ) ) {
2021-05-04 08:12:23 -03:00
Vector3f dVelF ;
float dVel_dtF ;
ins . get_delta_velocity ( ins_index , dVelF , dVel_dtF ) ;
dVel = dVelF . toftype ( ) ;
dVel_dt = dVel_dtF ;
2021-02-11 17:34:38 -04:00
dVel_dt = MAX ( dVel_dt , 1.0e-4 f ) ;
2017-12-04 16:49:19 -04:00
dVel_dt = MIN ( dVel_dt , 1.0e-1 f ) ;
2015-10-06 18:20:43 -03:00
return true ;
}
return false ;
}
/********************************************************
* Global Position Measurement *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// check for new valid GPS data and update stored measurement if available
void NavEKF2_core : : readGpsData ( )
{
2019-08-23 02:30:33 -03:00
if ( frontend - > _fusionModeGPS = = 3 ) {
// don't read GPS data if GPS usage disabled
return ;
}
2015-10-06 18:20:43 -03:00
// check for new GPS data
2015-10-20 23:46:36 -03:00
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
2020-11-07 02:24:13 -04:00
const auto & gps = dal . gps ( ) ;
2020-11-05 19:30:16 -04:00
if ( gps . last_message_time_ms ( gps . primary_sensor ( ) ) - lastTimeGpsReceived_ms > 70 ) {
if ( gps . status ( ) > = AP_DAL_GPS : : GPS_OK_FIX_3D ) {
2015-10-08 20:24:53 -03:00
// report GPS fix status
gpsCheckStatus . bad_fix = false ;
// store fix time from previous read
2020-10-21 02:13:00 -03:00
const uint32_t secondLastGpsTime_ms = lastTimeGpsReceived_ms ;
2015-10-08 20:24:53 -03:00
// get current fix time
2020-11-05 19:30:16 -04:00
lastTimeGpsReceived_ms = gps . last_message_time_ms ( gps . primary_sensor ( ) ) ;
2015-10-08 20:24:53 -03:00
2018-06-28 19:37:38 -03:00
2015-10-08 20:24:53 -03:00
// estimate when the GPS fix was valid, allowing for GPS processing and other delays
// ideally we should be using a timing signal from the GPS receiver to set this time
2018-06-28 19:37:38 -03:00
float gps_delay = 0.0 ;
gps . get_lag ( gps_delay ) ; // ignore the return value
gpsDataNew . time_ms = lastTimeGpsReceived_ms - ( uint32_t ) ( 1e3 f * gps_delay ) ;
2015-10-08 20:24:53 -03:00
2015-11-09 20:25:44 -04:00
// Correct for the average intersampling delay due to the filter updaterate
gpsDataNew . time_ms - = localFilterTimeStep_ms / 2 ;
2015-10-15 09:01:04 -03:00
2015-10-18 19:11:58 -03:00
// Prevent time delay exceeding age of oldest IMU data in the buffer
2015-11-27 13:11:58 -04:00
gpsDataNew . time_ms = MAX ( gpsDataNew . time_ms , imuDataDelayed . time_ms ) ;
2015-10-18 19:11:58 -03:00
2016-10-27 01:47:26 -03:00
// Get which GPS we are using for position information
2017-12-01 21:02:33 -04:00
gpsDataNew . sensor_idx = gps . primary_sensor ( ) ;
2016-10-11 18:27:01 -03:00
2015-10-08 20:24:53 -03:00
// read the NED velocity from the GPS
2021-05-04 08:12:23 -03:00
gpsDataNew . vel = gps . velocity ( ) . toftype ( ) ;
2015-10-08 20:24:53 -03:00
2016-05-06 19:49:36 -03:00
// Use the speed and position accuracy from the GPS if available, otherwise set it to zero.
// Apply a decaying envelope filter with a 5 second time constant to the raw accuracy data
2021-05-04 08:12:23 -03:00
ftype alpha = constrain_ftype ( 0.0002f * ( lastTimeGpsReceived_ms - secondLastGpsTime_ms ) , 0.0f , 1.0f ) ;
2015-10-08 20:24:53 -03:00
gpsSpdAccuracy * = ( 1.0f - alpha ) ;
float gpsSpdAccRaw ;
2017-12-01 21:02:33 -04:00
if ( ! gps . speed_accuracy ( gpsSpdAccRaw ) ) {
2015-10-08 20:24:53 -03:00
gpsSpdAccuracy = 0.0f ;
} else {
2015-11-27 13:11:58 -04:00
gpsSpdAccuracy = MAX ( gpsSpdAccuracy , gpsSpdAccRaw ) ;
2016-05-06 19:49:36 -03:00
gpsSpdAccuracy = MIN ( gpsSpdAccuracy , 50.0f ) ;
2020-08-20 01:10:04 -03:00
gpsSpdAccuracy = MAX ( gpsSpdAccuracy , frontend - > _gpsHorizVelNoise ) ;
2016-05-06 19:49:36 -03:00
}
gpsPosAccuracy * = ( 1.0f - alpha ) ;
float gpsPosAccRaw ;
2017-12-01 21:02:33 -04:00
if ( ! gps . horizontal_accuracy ( gpsPosAccRaw ) ) {
2016-05-06 19:49:36 -03:00
gpsPosAccuracy = 0.0f ;
} else {
gpsPosAccuracy = MAX ( gpsPosAccuracy , gpsPosAccRaw ) ;
gpsPosAccuracy = MIN ( gpsPosAccuracy , 100.0f ) ;
2020-08-20 01:10:04 -03:00
gpsPosAccuracy = MAX ( gpsPosAccuracy , frontend - > _gpsHorizPosNoise ) ;
2015-10-08 20:24:53 -03:00
}
2016-07-12 05:56:58 -03:00
gpsHgtAccuracy * = ( 1.0f - alpha ) ;
float gpsHgtAccRaw ;
2017-12-01 21:02:33 -04:00
if ( ! gps . vertical_accuracy ( gpsHgtAccRaw ) ) {
2016-07-12 05:56:58 -03:00
gpsHgtAccuracy = 0.0f ;
} else {
gpsHgtAccuracy = MAX ( gpsHgtAccuracy , gpsHgtAccRaw ) ;
gpsHgtAccuracy = MIN ( gpsHgtAccuracy , 100.0f ) ;
2020-08-20 01:10:04 -03:00
gpsHgtAccuracy = MAX ( gpsHgtAccuracy , 1.5f * frontend - > _gpsHorizPosNoise ) ;
2016-07-12 05:56:58 -03:00
}
2015-10-06 18:20:43 -03:00
2015-10-08 20:24:53 -03:00
// check if we have enough GPS satellites and increase the gps noise scaler if we don't
2017-12-01 21:02:33 -04:00
if ( gps . num_sats ( ) > = 6 & & ( PV_AidingMode = = AID_ABSOLUTE ) ) {
2015-10-08 20:24:53 -03:00
gpsNoiseScaler = 1.0f ;
2017-12-01 21:02:33 -04:00
} else if ( gps . num_sats ( ) = = 5 & & ( PV_AidingMode = = AID_ABSOLUTE ) ) {
2015-10-08 20:24:53 -03:00
gpsNoiseScaler = 1.4f ;
} else { // <= 4 satellites or in constant position mode
gpsNoiseScaler = 2.0f ;
}
2015-10-06 18:20:43 -03:00
2017-07-24 22:35:03 -03:00
// Check if GPS can output vertical velocity, if it is allowed to be used, and set GPS fusion mode accordingly
2020-11-20 15:38:49 -04:00
if ( gps . have_vertical_velocity ( ) & & frontend - > _fusionModeGPS = = 0 ) {
2015-10-08 20:24:53 -03:00
useGpsVertVel = true ;
} else {
useGpsVertVel = false ;
}
2015-10-06 18:20:43 -03:00
2019-07-01 19:49:48 -03:00
// Monitor quality of the GPS velocity data both before and after alignment. This updates
// GpsGoodToAlign class variable
calcGpsGoodToAlign ( ) ;
2015-10-06 18:20:43 -03:00
2016-03-30 22:59:59 -03:00
// Post-alignment checks
calcGpsGoodForFlight ( ) ;
2019-07-27 04:00:10 -03:00
// see if we can get origin from frontend
if ( ! validOrigin & & frontend - > common_origin_valid ) {
2021-06-16 15:13:48 -03:00
if ( ! setOrigin ( frontend - > common_EKF_origin ) ) {
// set an error as an attempt was made to set the origin more than once
INTERNAL_ERROR ( AP_InternalError : : error_t : : flow_of_control ) ;
return ;
}
2019-07-27 04:00:10 -03:00
}
2018-05-07 13:42:11 -03:00
// Read the GPS location in WGS-84 lat,long,height coordinates
2023-02-02 18:58:39 -04:00
const Location & gpsloc = gps . location ( ) ;
2015-11-05 18:48:21 -04:00
// Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
if ( gpsGoodToAlign & & ! validOrigin ) {
2021-06-18 00:08:02 -03:00
Location gpsloc_fieldelevation = gpsloc ;
// if flying, correct for height change from takeoff so that the origin is at field elevation
if ( inFlight ) {
gpsloc_fieldelevation . alt + = ( int32_t ) ( 100.0f * stateStruct . position . z ) ;
}
2021-06-16 15:13:48 -03:00
2021-06-18 00:08:02 -03:00
if ( ! setOrigin ( gpsloc_fieldelevation ) ) {
2021-06-16 15:13:48 -03:00
// set an error as an attempt was made to set the origin more than once
INTERNAL_ERROR ( AP_InternalError : : error_t : : flow_of_control ) ;
return ;
}
2016-07-04 10:54:28 -03:00
// set the NE earth magnetic field states using the published declination
// and set the corresponding variances and covariances
2015-11-06 02:35:12 -04:00
alignMagStateDeclination ( ) ;
2016-07-04 10:54:28 -03:00
2017-05-09 03:30:58 -03:00
// Set the height of the NED origin
ekfGpsRefHgt = ( double ) 0.01 * ( double ) gpsloc . alt + ( double ) outputDataNew . position . z ;
2016-07-04 10:54:28 -03:00
2018-05-07 13:42:11 -03:00
// Set the uncertainty of the GPS origin height
2016-07-12 05:56:58 -03:00
ekfOriginHgtVar = sq ( gpsHgtAccuracy ) ;
2015-11-05 18:48:21 -04:00
}
2019-06-01 08:03:16 -03:00
if ( gpsGoodToAlign & & ! have_table_earth_field ) {
2021-07-25 21:32:12 -03:00
const auto & compass = dal . compass ( ) ;
if ( compass . have_scale_factor ( magSelectIndex ) & &
compass . auto_declination_enabled ( ) ) {
2021-05-04 08:12:23 -03:00
table_earth_field_ga = AP_Declination : : get_earth_field_ga ( gpsloc ) . toftype ( ) ;
2019-11-26 18:13:14 -04:00
table_declination = radians ( AP_Declination : : get_declination ( gpsloc . lat * 1.0e-7 ,
gpsloc . lng * 1.0e-7 ) ) ;
have_table_earth_field = true ;
if ( frontend - > _mag_ef_limit > 0 ) {
// initialise earth field from tables
stateStruct . earth_magfield = table_earth_field_ga ;
}
2019-06-01 08:03:16 -03:00
}
}
2015-11-05 18:48:21 -04:00
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
2015-10-08 20:24:53 -03:00
if ( validOrigin ) {
2021-05-04 08:12:23 -03:00
gpsDataNew . pos = EKF_origin . get_distance_NE_ftype ( gpsloc ) ;
2019-07-09 03:27:22 -03:00
if ( ( frontend - > _originHgtMode & ( 1 < < 2 ) ) = = 0 ) {
gpsDataNew . hgt = ( float ) ( ( double ) 0.01 * ( double ) gpsloc . alt - ekfGpsRefHgt ) ;
} else {
gpsDataNew . hgt = 0.01 * ( gpsloc . alt - EKF_origin . alt ) ;
}
2015-11-20 18:05:12 -04:00
storedGPS . push ( gpsDataNew ) ;
2015-11-05 18:48:21 -04:00
// declare GPS available for use
gpsNotAvailable = false ;
2015-10-06 18:20:43 -03:00
}
2015-10-08 20:24:53 -03:00
} else {
// report GPS fix status
gpsCheckStatus . bad_fix = true ;
2020-11-07 02:24:13 -04:00
dal . snprintf ( prearm_fail_string , sizeof ( prearm_fail_string ) , " Waiting for 3D fix " ) ;
2015-10-08 20:24:53 -03:00
}
2015-10-06 18:20:43 -03:00
}
}
2015-10-30 03:07:38 -03:00
// read the delta angle and corresponding time interval from the IMU
// return false if data is not available
2021-05-04 08:12:23 -03:00
bool NavEKF2_core : : readDeltaAngle ( uint8_t ins_index , Vector3F & dAng , ftype & dAng_dt ) {
2020-11-07 02:24:13 -04:00
const auto & ins = dal . ins ( ) ;
2015-10-06 18:20:43 -03:00
if ( ins_index < ins . get_gyro_count ( ) ) {
2021-05-04 08:12:23 -03:00
Vector3f dAngF ;
float dAng_dtF ;
ins . get_delta_angle ( ins_index , dAngF , dAng_dtF ) ;
dAng = dAngF . toftype ( ) ;
dAng_dt = dAng_dtF ;
2021-02-11 17:34:38 -04:00
dAng_dt = MAX ( dAng_dt , 1.0e-4 f ) ;
2017-12-04 16:49:19 -04:00
dAng_dt = MIN ( dAng_dt , 1.0e-1 f ) ;
2015-10-06 18:20:43 -03:00
return true ;
}
return false ;
}
/********************************************************
* Height Measurements *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2015-11-12 04:07:52 -04:00
// check for new pressure altitude measurement data and update stored measurement if available
void NavEKF2_core : : readBaroData ( )
2015-10-06 18:20:43 -03:00
{
// check to see if baro measurement has changed so we know if a new measurement has arrived
2015-10-20 23:46:36 -03:00
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
2020-11-07 02:24:13 -04:00
const auto & baro = dal . baro ( ) ;
2018-03-05 16:35:34 -04:00
if ( baro . get_last_update ( ) - lastBaroReceived_ms > 70 ) {
2015-11-12 04:07:52 -04:00
2018-03-05 16:35:34 -04:00
baroDataNew . hgt = baro . get_altitude ( ) ;
2015-10-06 18:20:43 -03:00
2015-11-12 04:07:52 -04:00
// If we are in takeoff mode, the height measurement is limited to be no less than the measurement at start of takeoff
// This prevents negative baro disturbances due to copter downwash corrupting the EKF altitude during initial ascent
2021-05-28 23:01:16 -03:00
if ( dal . get_takeoff_expected ( ) & & ! assume_zero_sideslip ( ) ) {
2015-11-27 13:11:58 -04:00
baroDataNew . hgt = MAX ( baroDataNew . hgt , meaHgtAtTakeOff ) ;
2015-10-06 18:20:43 -03:00
}
// time stamp used to check for new measurement
2018-03-05 16:35:34 -04:00
lastBaroReceived_ms = baro . get_last_update ( ) ;
2015-10-06 18:20:43 -03:00
// estimate of time height measurement was taken, allowing for delays
2015-11-12 04:07:52 -04:00
baroDataNew . time_ms = lastBaroReceived_ms - frontend - > _hgtDelay_ms ;
2015-10-06 18:20:43 -03:00
2015-11-09 20:25:44 -04:00
// Correct for the average intersampling delay due to the filter updaterate
baroDataNew . time_ms - = localFilterTimeStep_ms / 2 ;
2015-10-18 19:11:58 -03:00
// Prevent time delay exceeding age of oldest IMU data in the buffer
2015-11-27 13:11:58 -04:00
baroDataNew . time_ms = MAX ( baroDataNew . time_ms , imuDataDelayed . time_ms ) ;
2015-10-15 09:01:04 -03:00
2015-10-06 18:20:43 -03:00
// save baro measurement to buffer to be fused later
2015-11-20 18:05:12 -04:00
storedBaro . push ( baroDataNew ) ;
2015-10-06 18:20:43 -03:00
}
}
2015-11-12 03:29:53 -04:00
// calculate filtered offset between baro height measurement and EKF height estimate
// offset should be subtracted from baro measurement to match filter estimate
2016-07-12 05:56:58 -03:00
// offset is used to enable reversion to baro from alternate height data source
2015-11-12 03:29:53 -04:00
void NavEKF2_core : : calcFiltBaroOffset ( )
{
// Apply a first order LPF with spike protection
2021-05-04 08:12:23 -03:00
baroHgtOffset + = 0.1f * constrain_ftype ( baroDataDelayed . hgt + stateStruct . position . z - baroHgtOffset , - 5.0f , 5.0f ) ;
2015-11-12 03:29:53 -04:00
}
2015-11-25 18:21:48 -04:00
2017-05-09 03:30:58 -03:00
// correct the height of the EKF origin to be consistent with GPS Data using a Bayes filter.
void NavEKF2_core : : correctEkfOriginHeight ( )
2016-07-12 05:56:58 -03:00
{
// Estimate the WGS-84 height of the EKF's origin using a Bayes filter
// calculate the variance of our a-priori estimate of the ekf origin height
2021-05-04 08:12:23 -03:00
ftype deltaTime = constrain_ftype ( 0.001f * ( imuDataDelayed . time_ms - lastOriginHgtTime_ms ) , 0.0f , 1.0f ) ;
2016-07-12 05:56:58 -03:00
if ( activeHgtSource = = HGT_SOURCE_BARO ) {
// Use the baro drift rate
2021-05-04 08:12:23 -03:00
const ftype baroDriftRate = 0.05f ;
2016-07-12 05:56:58 -03:00
ekfOriginHgtVar + = sq ( baroDriftRate * deltaTime ) ;
} else if ( activeHgtSource = = HGT_SOURCE_RNG ) {
// use the worse case expected terrain gradient and vehicle horizontal speed
2021-05-04 08:12:23 -03:00
const ftype maxTerrGrad = 0.25f ;
2021-09-11 07:33:42 -03:00
ekfOriginHgtVar + = sq ( maxTerrGrad * stateStruct . velocity . xy ( ) . length ( ) * deltaTime ) ;
2017-05-09 03:30:58 -03:00
} else {
// by definition our height source is absolute so cannot run this filter
2016-07-12 05:56:58 -03:00
return ;
}
lastOriginHgtTime_ms = imuDataDelayed . time_ms ;
2017-05-01 07:14:33 -03:00
// calculate the observation variance assuming EKF error relative to datum is independent of GPS observation error
2016-07-12 05:56:58 -03:00
// when not using GPS as height source
2021-05-04 08:12:23 -03:00
ftype originHgtObsVar = sq ( gpsHgtAccuracy ) + P [ 8 ] [ 8 ] ;
2016-07-12 05:56:58 -03:00
// calculate the correction gain
2021-05-04 08:12:23 -03:00
ftype gain = ekfOriginHgtVar / ( ekfOriginHgtVar + originHgtObsVar ) ;
2016-07-12 05:56:58 -03:00
// calculate the innovation
2021-05-04 08:12:23 -03:00
ftype innovation = - stateStruct . position . z - gpsDataDelayed . hgt ;
2016-07-12 05:56:58 -03:00
// check the innovation variance ratio
2021-05-04 08:12:23 -03:00
ftype ratio = sq ( innovation ) / ( ekfOriginHgtVar + originHgtObsVar ) ;
2016-07-12 05:56:58 -03:00
2017-05-09 03:30:58 -03:00
// correct the EKF origin and variance estimate if the innovation is less than 5-sigma
if ( ratio < 25.0f & & gpsAccuracyGood ) {
ekfGpsRefHgt - = ( double ) ( gain * innovation ) ;
ekfOriginHgtVar - = MAX ( gain * ekfOriginHgtVar , 0.0f ) ;
2016-07-12 05:56:58 -03:00
}
}
2015-10-06 18:20:43 -03:00
/********************************************************
* Air Speed Measurements *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// check for new airspeed data and update stored measurements if available
void NavEKF2_core : : readAirSpdData ( )
{
// if airspeed reading is valid and is set by the user to be used and has been updated then
// we take a new reading, convert from EAS to TAS and set the flag letting other functions
// know a new measurement is available
2020-11-07 02:24:13 -04:00
const auto * aspeed = dal . airspeed ( ) ;
2015-10-06 18:20:43 -03:00
if ( aspeed & &
2020-12-07 23:04:03 -04:00
aspeed - > use ( ) & &
aspeed - > healthy ( ) & &
aspeed - > last_update_ms ( ) ! = timeTasReceived_ms ) {
2020-11-07 02:24:13 -04:00
tasDataNew . tas = aspeed - > get_airspeed ( ) * dal . get_EAS2TAS ( ) ;
2015-10-06 18:20:43 -03:00
timeTasReceived_ms = aspeed - > last_update_ms ( ) ;
2015-11-04 21:00:57 -04:00
tasDataNew . time_ms = timeTasReceived_ms - frontend - > tasDelay_ms ;
2015-11-25 18:21:48 -04:00
2015-11-09 20:25:44 -04:00
// Correct for the average intersampling delay due to the filter update rate
tasDataNew . time_ms - = localFilterTimeStep_ms / 2 ;
2015-11-25 18:21:48 -04:00
// Save data into the buffer to be fused when the fusion time horizon catches up with it
2015-11-20 18:05:12 -04:00
storedTAS . push ( tasDataNew ) ;
2015-10-06 18:20:43 -03:00
}
2015-11-25 18:21:48 -04:00
// Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
tasDataToFuse = storedTAS . recall ( tasDataDelayed , imuDataDelayed . time_ms ) ;
2015-10-06 18:20:43 -03:00
}
2016-10-25 17:54:29 -03:00
/********************************************************
* Range Beacon Measurements *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
2022-11-13 18:50:01 -04:00
# if AP_BEACON_ENABLED
2017-05-01 07:14:33 -03:00
// check for new range beacon data and push to data buffer if available
2016-10-25 17:54:29 -03:00
void NavEKF2_core : : readRngBcnData ( )
{
// get the location of the beacon data
2020-11-07 02:24:13 -04:00
const auto * beacon = dal . beacon ( ) ;
2016-10-25 17:54:29 -03:00
2016-11-23 08:21:21 -04:00
// exit immediately if no beacon object
if ( beacon = = nullptr ) {
return ;
}
2016-10-25 17:54:29 -03:00
// get the number of beacons in use
N_beacons = beacon - > count ( ) ;
// search through all the beacons for new data and if we find it stop searching and push the data into the observation buffer
bool newDataToPush = false ;
uint8_t numRngBcnsChecked = 0 ;
// start the search one index up from where we left it last time
uint8_t index = lastRngBcnChecked ;
while ( ! newDataToPush & & numRngBcnsChecked < N_beacons ) {
// track the number of beacons checked
numRngBcnsChecked + + ;
// move to next beacon, wrap index if necessary
index + + ;
if ( index > = N_beacons ) {
index = 0 ;
}
// check that the beacon is healthy and has new data
2016-12-01 01:52:47 -04:00
if ( beacon - > beacon_healthy ( index ) & &
2016-10-25 17:54:29 -03:00
beacon - > beacon_last_update_ms ( index ) ! = lastTimeRngBcn_ms [ index ] )
{
// set the timestamp, correcting for measurement delay and average intersampling delay due to the filter update rate
lastTimeRngBcn_ms [ index ] = beacon - > beacon_last_update_ms ( index ) ;
rngBcnDataNew . time_ms = lastTimeRngBcn_ms [ index ] - frontend - > _rngBcnDelay_ms - localFilterTimeStep_ms / 2 ;
// set the range noise
// TODO the range library should provide the noise/accuracy estimate for each beacon
rngBcnDataNew . rngErr = frontend - > _rngBcnNoise ;
// set the range measurement
rngBcnDataNew . rng = beacon - > beacon_distance ( index ) ;
// set the beacon position
2021-05-04 08:12:23 -03:00
rngBcnDataNew . beacon_posNED = beacon - > beacon_position ( index ) . toftype ( ) ;
2016-10-25 17:54:29 -03:00
// identify the beacon identifier
rngBcnDataNew . beacon_ID = index ;
// indicate we have new data to push to the buffer
newDataToPush = true ;
// update the last checked index
lastRngBcnChecked = index ;
}
}
// Check if the beacon system has returned a 3D fix
2021-05-04 08:12:23 -03:00
Vector3f beaconVehiclePosNEDF ;
float beaconVehiclePosErrF ;
if ( beacon - > get_vehicle_position_ned ( beaconVehiclePosNEDF , beaconVehiclePosErrF ) ) {
2016-10-25 17:54:29 -03:00
rngBcnLast3DmeasTime_ms = imuSampleTime_ms ;
}
2021-05-04 08:12:23 -03:00
beaconVehiclePosNED = beaconVehiclePosNEDF . toftype ( ) ;
beaconVehiclePosErr = beaconVehiclePosErrF ;
2016-10-25 17:54:29 -03:00
// Check if the range beacon data can be used to align the vehicle position
if ( imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 & & beaconVehiclePosErr < 1.0f & & rngBcnAlignmentCompleted ) {
// check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
2021-05-04 08:12:23 -03:00
ftype posDiffSq = sq ( receiverPos . x - beaconVehiclePosNED . x ) + sq ( receiverPos . y - beaconVehiclePosNED . y ) ;
ftype posDiffVar = sq ( beaconVehiclePosErr ) + receiverPosCov [ 0 ] [ 0 ] + receiverPosCov [ 1 ] [ 1 ] ;
2016-10-25 17:54:29 -03:00
if ( posDiffSq < 9.0f * posDiffVar ) {
rngBcnGoodToAlign = true ;
// Set the EKF origin and magnetic field declination if not previously set
if ( ! validOrigin & & PV_AidingMode ! = AID_ABSOLUTE ) {
// get origin from beacon system
Location origin_loc ;
if ( beacon - > get_origin ( origin_loc ) ) {
setOriginLLH ( origin_loc ) ;
// set the NE earth magnetic field states using the published declination
// and set the corresponding variances and covariances
alignMagStateDeclination ( ) ;
// Set the uncertainty of the origin height
ekfOriginHgtVar = sq ( beaconVehiclePosErr ) ;
}
}
} else {
rngBcnGoodToAlign = false ;
}
} else {
rngBcnGoodToAlign = false ;
}
// Save data into the buffer to be fused when the fusion time horizon catches up with it
if ( newDataToPush ) {
storedRangeBeacon . push ( rngBcnDataNew ) ;
}
// Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
rngBcnDataToFuse = storedRangeBeacon . recall ( rngBcnDataDelayed , imuDataDelayed . time_ms ) ;
}
2022-11-13 18:50:01 -04:00
# endif // AP_BEACON_ENABLED
2017-04-27 21:49:58 -03:00
/*
update timing statistics structure
*/
void NavEKF2_core : : updateTimingStatistics ( void )
{
if ( timing . count = = 0 ) {
timing . dtIMUavg_max = dtIMUavg ;
timing . dtIMUavg_min = dtIMUavg ;
2017-04-28 04:14:22 -03:00
timing . dtEKFavg_max = dtEkfAvg ;
timing . dtEKFavg_min = dtEkfAvg ;
2017-04-27 21:49:58 -03:00
timing . delAngDT_max = imuDataDelayed . delAngDT ;
timing . delAngDT_min = imuDataDelayed . delAngDT ;
timing . delVelDT_max = imuDataDelayed . delVelDT ;
timing . delVelDT_min = imuDataDelayed . delVelDT ;
} else {
timing . dtIMUavg_max = MAX ( timing . dtIMUavg_max , dtIMUavg ) ;
timing . dtIMUavg_min = MIN ( timing . dtIMUavg_min , dtIMUavg ) ;
2017-04-28 04:14:22 -03:00
timing . dtEKFavg_max = MAX ( timing . dtEKFavg_max , dtEkfAvg ) ;
timing . dtEKFavg_min = MIN ( timing . dtEKFavg_min , dtEkfAvg ) ;
2017-04-27 21:49:58 -03:00
timing . delAngDT_max = MAX ( timing . delAngDT_max , imuDataDelayed . delAngDT ) ;
timing . delAngDT_min = MIN ( timing . delAngDT_min , imuDataDelayed . delAngDT ) ;
timing . delVelDT_max = MAX ( timing . delVelDT_max , imuDataDelayed . delVelDT ) ;
timing . delVelDT_min = MIN ( timing . delVelDT_min , imuDataDelayed . delVelDT ) ;
}
timing . count + + ;
}
2020-04-13 02:02:14 -03:00
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 )
2018-03-07 00:42:31 -04:00
{
2020-06-14 23:25:47 -03:00
// protect against NaN
if ( pos . is_nan ( ) | | isnan ( posErr ) | | quat . is_nan ( ) | | isnan ( angErr ) ) {
return ;
}
2018-03-07 00:42:31 -04:00
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
2020-06-08 00:13:13 -03:00
if ( ( timeStamp_ms - extNavMeasTime_ms ) < 20 ) {
2018-03-07 00:42:31 -04:00
return ;
} else {
extNavMeasTime_ms = timeStamp_ms ;
}
2020-04-09 23:24:02 -03:00
if ( resetTime_ms ! = extNavLastPosResetTime_ms ) {
2018-03-07 00:42:31 -04:00
extNavDataNew . posReset = true ;
extNavLastPosResetTime_ms = resetTime_ms ;
} else {
extNavDataNew . posReset = false ;
}
2021-05-04 08:12:23 -03:00
extNavDataNew . pos = pos . toftype ( ) ;
extNavDataNew . quat = quat . toftype ( ) ;
2020-06-03 23:53:00 -03:00
extNavDataNew . posErr = posErr ;
2018-03-07 00:42:31 -04:00
extNavDataNew . angErr = angErr ;
2020-04-13 02:02:14 -03:00
timeStamp_ms = timeStamp_ms - delay_ms ;
2018-10-30 05:27:51 -03:00
// Correct for the average intersampling delay due to the filter updaterate
timeStamp_ms - = localFilterTimeStep_ms / 2 ;
// Prevent time delay exceeding age of oldest IMU data in the buffer
timeStamp_ms = MAX ( timeStamp_ms , imuDataDelayed . time_ms ) ;
2018-03-07 00:42:31 -04:00
extNavDataNew . time_ms = timeStamp_ms ;
storedExtNav . push ( extNavDataNew ) ;
}
2019-06-01 08:03:16 -03:00
/*
return declination in radians
*/
2021-05-04 08:12:23 -03:00
ftype NavEKF2_core : : MagDeclination ( void ) const
2019-06-01 08:03:16 -03:00
{
// if we are using the WMM tables then use the table declination
// to ensure consistency with the table mag field. Otherwise use
// the declination from the compass library
if ( have_table_earth_field & & frontend - > _mag_ef_limit > 0 ) {
return table_declination ;
}
if ( ! use_compass ( ) ) {
return 0 ;
}
2021-07-25 21:32:12 -03:00
return dal . compass ( ) . get_declination ( ) ;
2019-06-01 08:03:16 -03:00
}
2019-07-04 04:11:57 -03:00
/*
update estimates of inactive bias states . This keeps inactive IMUs
as hot - spares so we can switch to them without causing a jump in the
error
*/
void NavEKF2_core : : learnInactiveBiases ( void )
{
2020-11-12 20:32:18 -04:00
# if INS_MAX_INSTANCES == 1
inactiveBias [ 0 ] . gyro_bias = stateStruct . gyro_bias ;
inactiveBias [ 0 ] . gyro_scale = stateStruct . gyro_scale ;
inactiveBias [ 0 ] . accel_zbias = stateStruct . accel_zbias ;
# else
2020-11-07 02:24:13 -04:00
const auto & ins = dal . ins ( ) ;
2019-07-04 04:11:57 -03:00
// learn gyro biases
for ( uint8_t i = 0 ; i < INS_MAX_INSTANCES ; i + + ) {
if ( ! ins . use_gyro ( i ) ) {
// can't use this gyro
continue ;
}
if ( gyro_index_active = = i ) {
// use current estimates from main filter of gyro bias and scale
inactiveBias [ i ] . gyro_bias = stateStruct . gyro_bias ;
inactiveBias [ i ] . gyro_scale = stateStruct . gyro_scale ;
} else {
// get filtered gyro and use the difference between the
// corrected gyro on the active IMU and the inactive IMU
// to move the inactive bias towards the right value
2021-05-04 08:12:23 -03:00
Vector3F filtered_gyro_active = ins . get_gyro ( gyro_index_active ) . toftype ( ) - ( stateStruct . gyro_bias / dtEkfAvg ) ;
Vector3F filtered_gyro_inactive = ins . get_gyro ( i ) . toftype ( ) - ( inactiveBias [ i ] . gyro_bias / dtEkfAvg ) ;
Vector3F error = filtered_gyro_active - filtered_gyro_inactive ;
2019-07-04 04:11:57 -03:00
// prevent a single large error from contaminating bias estimate
2021-05-04 08:12:23 -03:00
const ftype bias_limit = radians ( 5 ) ;
error . x = constrain_ftype ( error . x , - bias_limit , bias_limit ) ;
error . y = constrain_ftype ( error . y , - bias_limit , bias_limit ) ;
error . z = constrain_ftype ( error . z , - bias_limit , bias_limit ) ;
2019-07-04 04:11:57 -03:00
// slowly bring the inactive gyro in line with the active gyro. This corrects a 5 deg/sec
// gyro bias error in around 1 minute
inactiveBias [ i ] . gyro_bias - = error * ( 1.0e-4 f * dtEkfAvg ) ;
}
}
// learn accel biases
for ( uint8_t i = 0 ; i < INS_MAX_INSTANCES ; i + + ) {
if ( ! ins . use_accel ( i ) ) {
// can't use this accel
continue ;
}
if ( accel_index_active = = i ) {
// use current estimate from main filter
inactiveBias [ i ] . accel_zbias = stateStruct . accel_zbias ;
} else {
// get filtered accel and use the difference between the
// corrected accel on the active IMU and the inactive IMU
// to move the inactive bias towards the right value
2021-05-04 08:12:23 -03:00
ftype filtered_accel_active = ins . get_accel ( accel_index_active ) . z - ( stateStruct . accel_zbias / dtEkfAvg ) ;
ftype filtered_accel_inactive = ins . get_accel ( i ) . z - ( inactiveBias [ i ] . accel_zbias / dtEkfAvg ) ;
ftype error = filtered_accel_active - filtered_accel_inactive ;
2019-07-04 04:11:57 -03:00
// prevent a single large error from contaminating bias estimate
2021-05-04 08:12:23 -03:00
const ftype bias_limit = 1 ; // m/s/s
error = constrain_ftype ( error , - bias_limit , bias_limit ) ;
2019-07-04 04:11:57 -03:00
// slowly bring the inactive accel in line with the active accel
// this learns 0.5m/s/s bias in about 1 minute
inactiveBias [ i ] . accel_zbias - = error * ( 1.0e-4 f * dtEkfAvg ) ;
}
}
2020-11-12 20:32:18 -04:00
# endif
2019-07-04 04:11:57 -03:00
}
2020-03-25 00:53:13 -03:00
// Writes the default equivalent airspeed in m/s to be used in forward flight if a measured airspeed is required and not available.
void NavEKF2_core : : writeDefaultAirSpeed ( float airspeed )
{
defaultAirSpeed = airspeed ;
}
2018-10-03 23:43:13 -03:00
void NavEKF2_core : : writeExtNavVelData ( const Vector3f & vel , float err , uint32_t timeStamp_ms , uint16_t delay_ms )
{
2020-06-14 23:25:47 -03:00
// protect against NaN
if ( vel . is_nan ( ) | | isnan ( err ) ) {
return ;
}
2020-06-08 00:13:13 -03:00
if ( ( timeStamp_ms - extNavVelMeasTime_ms ) < 20 ) {
2018-10-03 23:43:13 -03:00
return ;
}
extNavVelMeasTime_ms = timeStamp_ms ;
useExtNavVel = true ;
2021-05-04 08:12:23 -03:00
extNavVelNew . vel = vel . toftype ( ) ;
2018-10-03 23:43:13 -03:00
extNavVelNew . err = err ;
timeStamp_ms = timeStamp_ms - delay_ms ;
// Correct for the average intersampling delay due to the filter updaterate
timeStamp_ms - = localFilterTimeStep_ms / 2 ;
// Prevent time delay exceeding age of oldest IMU data in the buffer
timeStamp_ms = MAX ( timeStamp_ms , imuDataDelayed . time_ms ) ;
extNavVelNew . time_ms = timeStamp_ms ;
storedExtNavVel . push ( extNavVelNew ) ;
}