2015-10-06 18:20:43 -03:00
/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
# 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>
# 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 ;
// get theoretical correct range when the vehicle is on the ground
2015-11-04 21:00:57 -04:00
rngOnGnd = frontend - > _rng . ground_clearance_cm ( ) * 0.01f ;
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
if ( frontend - > _rng . status ( ) = = RangeFinder : : RangeFinder_Good ) {
rngMeasIndex + + ;
if ( rngMeasIndex > 2 ) {
rngMeasIndex = 0 ;
}
storedRngMeasTime_ms [ rngMeasIndex ] = imuSampleTime_ms - 25 ;
storedRngMeas [ rngMeasIndex ] = frontend - > _rng . distance_cm ( ) * 0.01f ;
2015-10-06 18:20:43 -03:00
}
2015-11-12 04:07:52 -04:00
// check for three fresh samples
2015-10-06 18:20:43 -03:00
bool sampleFresh [ 3 ] ;
for ( uint8_t index = 0 ; index < = 2 ; index + + ) {
sampleFresh [ index ] = ( imuSampleTime_ms - storedRngMeasTime_ms [ index ] ) < 500 ;
}
2015-11-12 04:07:52 -04:00
// find the median value if we have three fresh samples
2015-10-06 18:20:43 -03:00
if ( sampleFresh [ 0 ] & & sampleFresh [ 1 ] & & sampleFresh [ 2 ] ) {
if ( storedRngMeas [ 0 ] > storedRngMeas [ 1 ] ) {
minIndex = 1 ;
maxIndex = 0 ;
} else {
maxIndex = 0 ;
minIndex = 1 ;
}
if ( storedRngMeas [ 2 ] > storedRngMeas [ maxIndex ] ) {
midIndex = maxIndex ;
} else if ( storedRngMeas [ 2 ] < storedRngMeas [ minIndex ] ) {
midIndex = minIndex ;
} else {
midIndex = 2 ;
}
2015-11-12 04:07:52 -04:00
rangeDataNew . time_ms = storedRngMeasTime_ms [ midIndex ] ;
// limit the measured range to be no less than the on-ground range
2015-11-27 13:11:58 -04:00
rangeDataNew . rng = MAX ( storedRngMeas [ midIndex ] , rngOnGnd ) ;
2015-10-06 18:20:43 -03:00
rngValidMeaTime_ms = imuSampleTime_ms ;
2015-11-12 04:07:52 -04:00
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
2015-11-20 18:05:12 -04:00
storedRange . push ( rangeDataNew ) ;
2015-11-12 04:07:52 -04:00
} else if ( ! takeOffDetected ) {
// before takeoff we assume on-ground range value if there is no data
rangeDataNew . time_ms = imuSampleTime_ms ;
rangeDataNew . rng = rngOnGnd ;
2015-10-06 18:20:43 -03:00
rngValidMeaTime_ms = imuSampleTime_ms ;
2015-11-12 04:07:52 -04:00
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
2015-11-20 18:05:12 -04:00
storedRange . push ( rangeDataNew ) ;
2015-10-06 18:20:43 -03:00
}
}
}
// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF2_core : : writeOptFlowMeas ( uint8_t & rawFlowQuality , Vector2f & rawFlowRates , Vector2f & rawGyroRates , uint32_t & msecFlowMeas )
{
// 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 ;
}
// check for takeoff if relying on optical flow and zero measurements until takeoff detected
// if we haven't taken off - constrain position and velocity states
2015-11-04 21:00:57 -04:00
if ( frontend - > _fusionModeGPS = = 3 ) {
2015-10-06 18:20:43 -03:00
detectOptFlowTakeoff ( ) ;
}
// calculate rotation matrices at mid sample time for flow observations
stateStruct . quat . rotation_matrix ( Tbn_flow ) ;
Tnb_flow = Tbn_flow . transposed ( ) ;
// 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 ) {
// correct flow sensor rates for bias
omegaAcrossFlowTime . x = rawGyroRates . x - flowGyroBias . x ;
omegaAcrossFlowTime . y = rawGyroRates . y - flowGyroBias . y ;
// write uncorrected flow rate measurements that will be used by the focal length scale factor estimator
// 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)
// write flow rate measurements corrected for body rates
ofDataNew . flowRadXYcomp . x = ofDataNew . flowRadXY . x + omegaAcrossFlowTime . x ;
ofDataNew . flowRadXYcomp . y = ofDataNew . flowRadXY . y + omegaAcrossFlowTime . y ;
// 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 *
* * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * * */
// return magnetometer offsets
// return true if offsets are valid
bool NavEKF2_core : : getMagOffsets ( Vector3f & magOffsets ) const
{
// compass offsets are valid if we have finalised magnetic field initialisation and magnetic field learning is not prohibited and primary compass is valid
2015-11-08 05:41:08 -04:00
if ( firstMagYawInit & & ( frontend - > _magCal ! = 2 ) & & _ahrs - > get_compass ( ) - > healthy ( magSelectIndex ) ) {
magOffsets = _ahrs - > get_compass ( ) - > get_offsets ( magSelectIndex ) - stateStruct . body_magfield * 1000.0f ;
2015-10-06 18:20:43 -03:00
return true ;
} else {
2015-11-08 05:41:08 -04:00
magOffsets = _ahrs - > get_compass ( ) - > get_offsets ( magSelectIndex ) ;
2015-10-06 18:20:43 -03:00
return false ;
}
}
// check for new magnetometer data and update store measurements if available
void NavEKF2_core : : readMagData ( )
{
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 ) {
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 ;
hal . console - > printf ( " EKF2 IMU%u switching to compass %u \n " , ( unsigned ) imu_index , magSelectIndex ) ;
// 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 ( ) ;
}
2015-11-08 05:41:08 -04:00
}
}
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 ( )
{
const AP_InertialSensor & ins = _ahrs - > get_ins ( ) ;
// 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 ) ;
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 ) ;
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 ) ) {
readDeltaAngle ( imu_index , imuDataNew . delAng ) ;
} else {
readDeltaAngle ( ins . get_primary_gyro ( ) , imuDataNew . delAng ) ;
}
2016-01-16 00:41:34 -04:00
imuDataNew . delAngDT = MAX ( ins . get_delta_angle_dt ( imu_index ) , 1.0e-4 f ) ;
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
// remove gyro scale factor errors
imuDataNew . delAng . x = imuDataNew . delAng . x * stateStruct . gyro_scale . x ;
imuDataNew . delAng . y = imuDataNew . delAng . y * stateStruct . gyro_scale . y ;
imuDataNew . delAng . z = imuDataNew . delAng . z * stateStruct . gyro_scale . z ;
// remove sensor bias errors
2016-01-11 22:41:40 -04:00
imuDataNew . delAng - = stateStruct . gyro_bias * ( imuDataNew . delAngDT / dtEkfAvg ) ;
imuDataNew . delVel . z - = stateStruct . accel_zbias * ( imuDataNew . delVelDT / dtEkfAvg ) ;
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
Quaternion deltaQuat ;
deltaQuat . rotate ( imuDataNew . delAng ) ;
imuQuatDownSampleNew = imuQuatDownSampleNew * deltaQuat ;
imuQuatDownSampleNew . normalize ( ) ;
// Rotate the accumulated delta velocity into the new frame of reference created by the latest delta angle
// This prevents introduction of sculling errors due to downsampling
Matrix3f deltaRotMat ;
deltaQuat . inverse ( ) . rotation_matrix ( deltaRotMat ) ;
imuDataDownSampledNew . delVel = deltaRotMat * imuDataDownSampledNew . delVel ;
// accumulate the latest delta velocity
imuDataDownSampledNew . delVel + = imuDataNew . delVel ;
// Keep track of the number of IMU frames since the last state prediction
framesSincePredict + + ;
// If 10msec has elapsed, and the frontend has allowed us to start a new predict cycle, then store the accumulated IMU data
// to be used by the state prediction, ignoring the frontend permission if more than 20msec has lapsed
if ( ( dtIMUavg * ( float ) framesSincePredict > = 0.01f & & startPredictEnabled ) | | ( dtIMUavg * ( float ) framesSincePredict > = 0.02f ) ) {
// convert the accumulated quaternion to an equivalent delta angle
imuQuatDownSampleNew . to_axis_angle ( imuDataDownSampledNew . delAng ) ;
// Time stamp the data
imuDataDownSampledNew . time_ms = imuSampleTime_ms ;
// Write data to the FIFO IMU buffer
2015-11-20 18:05:12 -04:00
storedIMU . push_youngest_element ( imuDataDownSampledNew ) ;
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 ;
// reset the counter used to let the frontend know how many frames have elapsed since we started a new update cycle
framesSincePredict = 0 ;
// set the flag to let the filter know it has new IMU data nad needs to run
runUpdates = true ;
} 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
// extract the oldest available data from the FIFO buffer
2015-11-20 18:05:12 -04:00
imuDataDelayed = storedIMU . pop_oldest_element ( ) ;
2015-11-09 20:25:44 -04:00
float minDT = 0.1f * dtEkfAvg ;
2015-11-27 13:11:58 -04:00
imuDataDelayed . delAngDT = MAX ( imuDataDelayed . delAngDT , minDT ) ;
imuDataDelayed . delVelDT = MAX ( imuDataDelayed . delVelDT , minDT ) ;
2015-11-13 18:28:45 -04:00
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 ) {
const AP_InertialSensor & ins = _ahrs - > get_ins ( ) ;
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 ) ;
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
if ( _ahrs - > get_gps ( ) . last_message_time_ms ( ) - lastTimeGpsReceived_ms > 70 ) {
2015-10-08 20:24:53 -03:00
if ( _ahrs - > get_gps ( ) . status ( ) > = AP_GPS : : GPS_OK_FIX_3D ) {
// report GPS fix status
gpsCheckStatus . bad_fix = false ;
// store fix time from previous read
secondLastGpsTime_ms = lastTimeGpsReceived_ms ;
// get current fix time
lastTimeGpsReceived_ms = _ahrs - > get_gps ( ) . last_message_time_ms ( ) ;
// 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
2015-11-04 21:00:57 -04:00
gpsDataNew . time_ms = lastTimeGpsReceived_ms - frontend - > _gpsDelay_ms ;
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
2015-10-08 20:24:53 -03:00
// read the NED velocity from the GPS
gpsDataNew . vel = _ahrs - > get_gps ( ) . velocity ( ) ;
// Use the speed 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 speed accuracy data
float alpha = constrain_float ( 0.0002f * ( lastTimeGpsReceived_ms - secondLastGpsTime_ms ) , 0.0f , 1.0f ) ;
gpsSpdAccuracy * = ( 1.0f - alpha ) ;
float gpsSpdAccRaw ;
if ( ! _ahrs - > get_gps ( ) . speed_accuracy ( gpsSpdAccRaw ) ) {
gpsSpdAccuracy = 0.0f ;
} else {
2015-11-27 13:11:58 -04:00
gpsSpdAccuracy = MAX ( gpsSpdAccuracy , gpsSpdAccRaw ) ;
2015-10-08 20:24:53 -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
if ( _ahrs - > get_gps ( ) . num_sats ( ) > = 6 & & ( PV_AidingMode = = AID_ABSOLUTE ) ) {
gpsNoiseScaler = 1.0f ;
} else if ( _ahrs - > get_gps ( ) . num_sats ( ) = = 5 & & ( PV_AidingMode = = AID_ABSOLUTE ) ) {
gpsNoiseScaler = 1.4f ;
} else { // <= 4 satellites or in constant position mode
gpsNoiseScaler = 2.0f ;
}
2015-10-06 18:20:43 -03:00
2015-10-08 20:24:53 -03:00
// Check if GPS can output vertical velocity and set GPS fusion mode accordingly
2015-11-04 21:00:57 -04:00
if ( _ahrs - > get_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
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 ( ) ;
2015-10-08 20:24:53 -03:00
} else {
// Post-alignment checks
calcGpsGoodForFlight ( ) ;
}
2015-10-06 18:20:43 -03:00
2015-11-05 18:48:21 -04:00
// Read the GPS locaton in WGS-84 lat,long,height coordinates
2015-10-08 20:24:53 -03:00
const struct Location & gpsloc = _ahrs - > get_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 ( ) ;
// Now we know the location we have an estimate for the magnetic field declination and adjust the earth field accordingly
alignMagStateDeclination ( ) ;
// Set the height of the NED origin to ‘ height of baro height datum relative to GPS height datum'
EKF_origin . alt = gpsloc . alt - baroDataNew . hgt ;
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 ) ;
2015-11-12 04:07:52 -04:00
gpsDataNew . hgt = 0.01f * ( 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-11-05 18:48:21 -04:00
// Commence GPS aiding when able to
if ( readyToUseGPS ( ) & & PV_AidingMode ! = AID_ABSOLUTE ) {
PV_AidingMode = AID_ABSOLUTE ;
// Initialise EKF position and velocity states to last GPS measurement
ResetPosition ( ) ;
ResetVelocity ( ) ;
}
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 ;
}
2015-10-06 18:20:43 -03:00
}
// We need to handle the case where GPS is lost for a period of time that is too long to dead-reckon
// If that happens we need to put the filter into a constant position mode, reset the velocity states to zero
// and use the last estimated position as a synthetic GPS position
// check if we can use opticalflow as a backup
bool optFlowBackupAvailable = ( flowDataValid & & ! hgtTimeout ) ;
// Set GPS time-out threshold depending on whether we have an airspeed sensor to constrain drift
2015-11-04 21:00:57 -04:00
uint16_t gpsRetryTimeout_ms = useAirspeed ( ) ? frontend - > gpsRetryTimeUseTAS_ms : frontend - > gpsRetryTimeNoTAS_ms ;
2015-10-06 18:20:43 -03:00
// Set the time that copters will fly without a GPS lock before failing the GPS and switching to a non GPS mode
2015-11-04 21:00:57 -04:00
uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend - > gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms ;
2015-10-06 18:20:43 -03:00
// If we haven't received GPS data for a while and we are using it for aiding, then declare the position and velocity data as being timed out
if ( imuSampleTime_ms - lastTimeGpsReceived_ms > gpsFailTimeout_ms ) {
2015-10-29 03:57:56 -03:00
// Let other processes know that GPS is not available and that a timeout has occurred
2015-10-06 18:20:43 -03:00
posTimeout = true ;
velTimeout = true ;
gpsNotAvailable = true ;
2015-10-29 03:57:56 -03:00
// If we are totally reliant on GPS for navigation, then we need to switch to a non-GPS mode of operation
// If we don't have airspeed or sideslip assumption or optical flow to constrain drift, then go into constant position mode.
// If we can do optical flow nav (valid flow data and height above ground estimate), then go into flow nav mode.
if ( PV_AidingMode = = AID_ABSOLUTE & & ! useAirspeed ( ) & & ! assume_zero_sideslip ( ) ) {
if ( optFlowBackupAvailable ) {
// we can do optical flow only nav
2015-11-04 21:00:57 -04:00
frontend - > _fusionModeGPS = 3 ;
2015-10-29 03:57:56 -03:00
PV_AidingMode = AID_RELATIVE ;
} else {
// store the current position
lastKnownPositionNE . x = stateStruct . position . x ;
lastKnownPositionNE . y = stateStruct . position . y ;
2015-10-06 18:20:43 -03:00
2015-10-29 03:57:56 -03:00
// put the filter into constant position mode
PV_AidingMode = AID_NONE ;
2015-10-06 18:20:43 -03:00
2015-10-29 03:57:56 -03:00
// Reset the velocity and position states
ResetVelocity ( ) ;
ResetPosition ( ) ;
2015-10-06 18:20:43 -03:00
2015-10-29 07:46:26 -03:00
// Reset the normalised innovation to avoid false failing bad fusion tests
2015-10-29 03:57:56 -03:00
velTestRatio = 0.0f ;
posTestRatio = 0.0f ;
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
2015-10-06 18:20:43 -03:00
bool NavEKF2_core : : readDeltaAngle ( uint8_t ins_index , Vector3f & dAng ) {
const AP_InertialSensor & ins = _ahrs - > get_ins ( ) ;
if ( ins_index < ins . get_gyro_count ( ) ) {
ins . get_delta_angle ( ins_index , dAng ) ;
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
2015-11-12 04:07:52 -04:00
if ( frontend - > _baro . get_last_update ( ) - lastBaroReceived_ms > 70 ) {
baroDataNew . hgt = frontend - > _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
if ( isAiding & & 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
2015-11-12 04:07:52 -04:00
lastBaroReceived_ms = frontend - > _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
// offset is used to enable reversion to baro if alternate height data sources fail
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
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
}
2015-10-07 15:27:09 -03:00
# endif // HAL_CPU_CLASS