2015-10-06 18:20:43 -03:00
# include <AP_HAL/AP_HAL.h>
# if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
# include "AP_NavEKF2.h"
# include "AP_NavEKF2_core.h"
# include <AP_AHRS/AP_AHRS.h>
# include <AP_Vehicle/AP_Vehicle.h>
2016-07-18 23:16:25 -03:00
# include <GCS_MAVLink/GCS.h>
2017-08-08 03:06:48 -03:00
# include <AP_RangeFinder/RangeFinder_Backend.h>
2015-10-06 18:20:43 -03:00
# include <stdio.h>
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
rngOnGnd = MAX ( frontend - > _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
if ( ( imuSampleTime_ms - lastRngMeasTime_ms ) > 50 ) {
// 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
2016-07-12 05:56:58 -03:00
for ( uint8_t sensorIndex = 0 ; sensorIndex < = 1 ; sensorIndex + + ) {
2017-08-08 03:06:48 -03:00
AP_RangeFinder_Backend * sensor = frontend - > _rng . get_backend ( sensorIndex ) ;
if ( sensor = = nullptr ) {
continue ;
}
if ( ( sensor - > orientation ( ) = = ROTATION_PITCH_270 ) & & ( sensor - > status ( ) = = RangeFinder : : RangeFinder_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 ;
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
bool sampleFresh [ 2 ] [ 3 ] = { } ;
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 ;
rangeDataNew . time_ms = imuSampleTime_ms ;
// don't allow time to go backwards
if ( imuSampleTime_ms > rangeDataNew . time_ms ) {
rangeDataNew . time_ms = imuSampleTime_ms ;
}
// 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
}
}
}
// write the raw optical flow measurements
// this needs to be called externally.
2016-10-27 01:47:26 -03:00
void NavEKF2_core : : writeOptFlowMeas ( uint8_t & rawFlowQuality , Vector2f & rawFlowRates , Vector2f & rawGyroRates , uint32_t & msecFlowMeas , const Vector3f & posOffset )
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 ) {
flowGyroBias . x = 0.99f * flowGyroBias . x + 0.01f * constrain_float ( ( rawGyroRates . x - delAngBodyOF . x / delTimeOF ) , - 0.1f , 0.1f ) ;
flowGyroBias . y = 0.99f * flowGyroBias . y + 0.01f * constrain_float ( ( rawGyroRates . y - delAngBodyOF . y / delTimeOF ) , - 0.1f , 0.1f ) ;
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
ofDataNew . flowRadXY = - rawFlowRates ; // 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
2016-10-27 01:47:26 -03:00
ofDataNew . body_offset = & posOffset ;
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 *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// check for new magnetometer data and update store measurements if available
void NavEKF2_core : : readMagData ( )
{
2016-05-19 01:16:40 -03:00
if ( ! _ahrs - > get_compass ( ) ) {
allMagSensorsFailed = true ;
return ;
}
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
uint8_t maxCount = _ahrs - > get_compass ( ) - > get_count ( ) ;
if ( allMagSensorsFailed | | ( magTimeout & & assume_zero_sideslip ( ) & & magSelectIndex > = maxCount - 1 & & inFlight ) ) {
allMagSensorsFailed = true ;
return ;
}
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
if ( use_compass ( ) & & _ahrs - > get_compass ( ) - > last_update_usec ( ) - lastMagUpdate_us > 70000 ) {
2016-05-03 19:23:51 -03:00
frontend - > logging . log_compass = true ;
2015-11-08 05:41:08 -04:00
// If the magnetometer has timed out (been rejected too long) we find another magnetometer to use if available
2015-11-08 22:50:27 -04:00
// 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 ( magTimeout & & ( maxCount > 1 ) & & ! onGround & & imuSampleTime_ms - ekfStartTime_ms > 30000 ) {
2015-11-15 17:05:08 -04:00
2015-11-08 05:41:08 -04:00
// 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 ( _ahrs - > get_compass ( ) - > use_for_yaw ( tempIndex ) & & tempIndex ! = magSelectIndex ) {
magSelectIndex = tempIndex ;
2017-07-09 01:16:08 -03:00
gcs ( ) . send_text ( MAV_SEVERITY_INFO , " EKF2 IMU%u switching to compass %u " , ( unsigned ) imu_index , magSelectIndex ) ;
2015-11-08 05:41:08 -04: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
2015-11-13 18:28:45 -04:00
storedMag . reset ( ) ;
2016-07-18 10:25:56 -03:00
// 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 ;
}
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
Vector3f nowMagOffsets = _ahrs - > get_compass ( ) - > get_offsets ( magSelectIndex ) ;
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 ( ) ;
}
lastMagOffsets = nowMagOffsets ;
lastMagOffsetsValid = true ;
2015-10-06 18:20:43 -03:00
// store time of last measurement update
2015-11-08 22:42:17 -04:00
lastMagUpdate_us = _ahrs - > get_compass ( ) - > last_update_usec ( magSelectIndex ) ;
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
2015-11-08 05:41:08 -04:00
magDataNew . mag = _ahrs - > get_compass ( ) - > get_field ( magSelectIndex ) * 0.001f ;
2015-10-06 18:20:43 -03:00
// check for consistent data between magnetometers
consistentMagData = _ahrs - > get_compass ( ) - > consistent ( ) ;
// save magnetometer measurement to buffer to be fused later
2015-11-20 18:05:12 -04:00
storedMag . push ( magDataNew ) ;
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 ( )
{
2018-03-10 05:35:50 -04:00
const AP_InertialSensor & ins = AP : : 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
// the imu sample time is used as a common time reference throughout the filter
2015-11-19 23:13:15 -04:00
imuSampleTime_ms = AP_HAL : : millis ( ) ;
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
if ( ins . use_accel ( imu_index ) ) {
readDeltaVelocity ( imu_index , imuDataNew . delVel , imuDataNew . delVelDT ) ;
2016-10-11 18:30:01 -03:00
accelPosOffset = ins . get_imu_pos_offset ( imu_index ) ;
2015-10-19 02:46:49 -03:00
} else {
2015-11-04 22:05:11 -04:00
readDeltaVelocity ( ins . get_primary_accel ( ) , imuDataNew . delVel , imuDataNew . delVelDT ) ;
2016-10-11 18:30:01 -03:00
accelPosOffset = ins . get_imu_pos_offset ( ins . get_primary_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
// Get delta angle data from primary gyro or primary if not available
if ( ins . use_gyro ( imu_index ) ) {
2017-12-04 04:51:06 -04:00
readDeltaAngle ( imu_index , imuDataNew . delAng , imuDataNew . delAngDT ) ;
2015-11-04 22:05:11 -04:00
} else {
2017-12-04 04:51:06 -04:00
readDeltaAngle ( ins . get_primary_gyro ( ) , imuDataNew . delAng , imuDataNew . delAngDT ) ;
2015-11-04 22:05:11 -04:00
}
2015-10-06 18:20:43 -03:00
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 ;
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
2015-11-09 20:25:44 -04: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
float dtNow = constrain_float ( 0.5f * ( imuDataDownSampledNew . delAngDT + imuDataDownSampledNew . delVelDT ) , 0.0f , 10.0f * EKF_TARGET_DT ) ;
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 ;
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
imuDataDelayed = storedIMU . pop_oldest_element ( ) ;
2016-06-29 09:38:45 -03:00
// protect against delta time going to zero
// TODO - check if calculations can tolerate 0
2016-06-15 07:20:50 -03:00
float minDT = 0.1f * dtEkfAvg ;
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 ;
correctDeltaAngle ( delAngCorrected , imuDataDelayed . delAngDT ) ;
correctDeltaVelocity ( delVelCorrected , imuDataDelayed . delVelDT ) ;
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
2015-10-06 18:20:43 -03:00
bool NavEKF2_core : : readDeltaVelocity ( uint8_t ins_index , Vector3f & dVel , float & dVel_dt ) {
2018-03-10 05:35:50 -04:00
const AP_InertialSensor & ins = AP : : ins ( ) ;
2015-10-06 18:20:43 -03:00
if ( ins_index < ins . get_accel_count ( ) ) {
ins . get_delta_velocity ( ins_index , dVel ) ;
2015-11-27 13:11:58 -04:00
dVel_dt = MAX ( ins . get_delta_velocity_dt ( ins_index ) , 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 ( )
{
// 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
2017-12-01 21:02:33 -04:00
const AP_GPS & gps = AP : : gps ( ) ;
if ( gps . last_message_time_ms ( ) - lastTimeGpsReceived_ms > 70 ) {
if ( gps . status ( ) > = AP_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
secondLastGpsTime_ms = lastTimeGpsReceived_ms ;
// get current fix time
2017-12-01 21:02:33 -04:00
lastTimeGpsReceived_ms = gps . last_message_time_ms ( ) ;
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
2017-12-01 21:02:33 -04:00
gpsDataNew . vel = gps . velocity ( ) ;
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
2015-10-08 20:24:53 -03:00
float alpha = constrain_float ( 0.0002f * ( lastTimeGpsReceived_ms - secondLastGpsTime_ms ) , 0.0f , 1.0f ) ;
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 ) ;
}
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 ) ;
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 ) ;
}
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
2017-12-01 21:02:33 -04:00
if ( gps . have_vertical_velocity ( ) & & frontend - > _fusionModeGPS = = 0 & & ! frontend - > inhibitGpsVertVelUse ) {
2015-10-08 20:24:53 -03:00
useGpsVertVel = true ;
} else {
useGpsVertVel = false ;
}
2015-10-06 18:20:43 -03:00
2015-10-08 20:24:53 -03:00
// Monitor quality of the GPS velocity data before and after alignment using separate checks
if ( PV_AidingMode ! = AID_ABSOLUTE ) {
// Pre-alignment checks
2015-10-20 05:49:13 -03:00
gpsGoodToAlign = calcGpsGoodToAlign ( ) ;
2016-07-17 01:27:37 -03:00
} else {
gpsGoodToAlign = false ;
2015-10-08 20:24:53 -03:00
}
2015-10-06 18:20:43 -03:00
2016-03-30 22:59:59 -03:00
// Post-alignment checks
calcGpsGoodForFlight ( ) ;
2018-05-07 13:42:11 -03:00
// Read the GPS location in WGS-84 lat,long,height coordinates
2017-12-01 21:02:33 -04:00
const struct 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 ) {
2015-11-06 02:35:12 -04:00
setOrigin ( ) ;
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
}
// 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 ) {
gpsDataNew . pos = location_diff ( EKF_origin , gpsloc ) ;
2017-05-09 03:30:58 -03:00
gpsDataNew . hgt = ( float ) ( ( double ) 0.01 * ( double ) gpsloc . alt - ekfGpsRefHgt ) ;
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
}
2016-05-03 19:23:51 -03:00
frontend - > logging . log_gps = true ;
2016-05-20 22:16:03 -03:00
2015-10-08 20:24:53 -03:00
} else {
// report GPS fix status
gpsCheckStatus . bad_fix = true ;
}
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
2017-12-04 04:51:06 -04:00
bool NavEKF2_core : : readDeltaAngle ( uint8_t ins_index , Vector3f & dAng , float & dAng_dt ) {
2018-03-10 05:35:50 -04:00
const AP_InertialSensor & ins = AP : : ins ( ) ;
2015-10-06 18:20:43 -03:00
if ( ins_index < ins . get_gyro_count ( ) ) {
ins . get_delta_angle ( ins_index , dAng ) ;
2016-05-03 19:23:51 -03:00
frontend - > logging . log_imu = true ;
2017-12-04 04:51:06 -04:00
dAng_dt = MAX ( ins . get_delta_angle_dt ( imu_index ) , 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
2018-03-05 16:35:34 -04:00
const AP_Baro & baro = AP : : baro ( ) ;
if ( baro . get_last_update ( ) - lastBaroReceived_ms > 70 ) {
2016-05-03 19:23:51 -03:00
frontend - > logging . log_baro = true ;
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
2016-07-15 00:09:48 -03:00
if ( getTakeoffExpected ( ) ) {
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
baroHgtOffset + = 0.1f * constrain_float ( baroDataDelayed . hgt + stateStruct . position . z - baroHgtOffset , - 5.0f , 5.0f ) ;
}
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
float deltaTime = constrain_float ( 0.001f * ( imuDataDelayed . time_ms - lastOriginHgtTime_ms ) , 0.0f , 1.0f ) ;
if ( activeHgtSource = = HGT_SOURCE_BARO ) {
// Use the baro drift rate
const float baroDriftRate = 0.05f ;
ekfOriginHgtVar + = sq ( baroDriftRate * deltaTime ) ;
} else if ( activeHgtSource = = HGT_SOURCE_RNG ) {
// use the worse case expected terrain gradient and vehicle horizontal speed
const float maxTerrGrad = 0.25f ;
ekfOriginHgtVar + = sq ( maxTerrGrad * norm ( stateStruct . velocity . x , stateStruct . velocity . y ) * 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
float originHgtObsVar = sq ( gpsHgtAccuracy ) + P [ 8 ] [ 8 ] ;
// calculate the correction gain
float gain = ekfOriginHgtVar / ( ekfOriginHgtVar + originHgtObsVar ) ;
// calculate the innovation
float innovation = - stateStruct . position . z - gpsDataDelayed . hgt ;
// check the innovation variance ratio
float ratio = sq ( innovation ) / ( ekfOriginHgtVar + originHgtObsVar ) ;
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
const AP_Airspeed * aspeed = _ahrs - > get_airspeed ( ) ;
if ( aspeed & &
aspeed - > use ( ) & &
aspeed - > last_update_ms ( ) ! = timeTasReceived_ms ) {
tasDataNew . tas = aspeed - > get_airspeed ( ) * aspeed - > get_EAS2TAS ( ) ;
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 *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
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
const AP_Beacon * beacon = _ahrs - > get_beacon ( ) ;
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
rngBcnDataNew . beacon_posNED = beacon - > beacon_position ( index ) ;
// 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
2016-12-01 01:52:47 -04:00
if ( beacon - > get_vehicle_position_ned ( beaconVehiclePosNED , beaconVehiclePosErr ) ) {
2016-10-25 17:54:29 -03:00
rngBcnLast3DmeasTime_ms = imuSampleTime_ms ;
}
// 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
float posDiffSq = sq ( receiverPos . x - beaconVehiclePosNED . x ) + sq ( receiverPos . y - beaconVehiclePosNED . y ) ;
float posDiffVar = sq ( beaconVehiclePosErr ) + receiverPosCov [ 0 ] [ 0 ] + receiverPosCov [ 1 ] [ 1 ] ;
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 ) ;
}
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 + + ;
}
// get timing statistics structure
void NavEKF2_core : : getTimingStatistics ( struct ekf_timing & _timing )
{
_timing = timing ;
memset ( & timing , 0 , sizeof ( timing ) ) ;
}
2018-03-07 00:42:31 -04:00
void NavEKF2_core : : writeExtNavData ( const Vector3f & sensOffset , const Vector3f & pos , const Quaternion & quat , float posErr , float angErr , uint32_t timeStamp_ms , uint32_t resetTime_ms )
{
// 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
if ( ( timeStamp_ms - extNavMeasTime_ms ) < 70 ) {
return ;
} else {
extNavMeasTime_ms = timeStamp_ms ;
}
if ( resetTime_ms > extNavLastPosResetTime_ms ) {
extNavDataNew . posReset = true ;
extNavLastPosResetTime_ms = resetTime_ms ;
} else {
extNavDataNew . posReset = false ;
}
extNavDataNew . pos = pos ;
extNavDataNew . quat = quat ;
extNavDataNew . posErr = posErr ;
extNavDataNew . angErr = angErr ;
extNavDataNew . body_offset = & sensOffset ;
extNavDataNew . time_ms = timeStamp_ms ;
storedExtNav . push ( extNavDataNew ) ;
}
2015-10-07 15:27:09 -03:00
# endif // HAL_CPU_CLASS