mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
e6f36f04db
The EK2_RNG_USE_HGT parameter sets the height (expressed as a percentage of the maximum range of the range finder as set by the RNGFND_MAX_CM parameter) below which the range finder will be used as the primary height source when the vehicle is moving slowly. When using a height reference other than GPS, the height datum can drift due to air pressure changes if using baro, or due to terrain height changes if using range finder as the primary height source. To ensure that a consistent height datum is available when switching between altitude sources, the WGS-84 height estimate of the EKF's local positi norigin is updated using a single state Bayes estimator, If rngfinder or gps height data is lost whilst being used, there will be a fall-back to baro data.
631 lines
29 KiB
C++
631 lines
29 KiB
C++
/// -*- 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 <GCS_MAVLink/GCS.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
|
||
// Apply a median filter
|
||
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
|
||
rngOnGnd = frontend->_rng.ground_clearance_cm() * 0.01f;
|
||
|
||
// 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
|
||
// use data from two range finders if available
|
||
|
||
for (uint8_t sensorIndex = 0; sensorIndex <= 1; sensorIndex++) {
|
||
if (frontend->_rng.status(sensorIndex) == RangeFinder::RangeFinder_Good) {
|
||
rngMeasIndex[sensorIndex] ++;
|
||
if (rngMeasIndex[sensorIndex] > 2) {
|
||
rngMeasIndex[sensorIndex] = 0;
|
||
}
|
||
storedRngMeasTime_ms[sensorIndex][rngMeasIndex[sensorIndex]] = imuSampleTime_ms - 25;
|
||
storedRngMeas[sensorIndex][rngMeasIndex[sensorIndex]] = frontend->_rng.distance_cm(sensorIndex) * 0.01f;
|
||
}
|
||
|
||
// 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;
|
||
}
|
||
|
||
// 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 {
|
||
maxIndex = 0;
|
||
minIndex = 1;
|
||
}
|
||
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);
|
||
|
||
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
|
||
storedRange.push(rangeDataNew);
|
||
|
||
} 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);
|
||
|
||
}
|
||
|
||
rngValidMeaTime_ms = imuSampleTime_ms;
|
||
|
||
}
|
||
}
|
||
}
|
||
|
||
// 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
|
||
if (frontend->_fusionModeGPS == 3) {
|
||
detectOptFlowTakeoff();
|
||
}
|
||
// 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) {
|
||
// 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
|
||
ofDataNew.time_ms = imuSampleTime_ms - frontend->_flowDelay_ms - frontend->flowTimeDeltaAvg_ms/2;
|
||
// Correct for the average intersampling delay due to the filter updaterate
|
||
ofDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||
ofDataNew.time_ms = MAX(ofDataNew.time_ms,imuDataDelayed.time_ms);
|
||
// Save data to buffer
|
||
storedOF.push(ofDataNew);
|
||
// Check for data at the fusion time horizon
|
||
flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
|
||
}
|
||
}
|
||
|
||
|
||
/********************************************************
|
||
* MAGNETOMETER *
|
||
********************************************************/
|
||
|
||
// check for new magnetometer data and update store measurements if available
|
||
void NavEKF2_core::readMagData()
|
||
{
|
||
if (!_ahrs->get_compass()) {
|
||
allMagSensorsFailed = true;
|
||
return;
|
||
}
|
||
// 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;
|
||
}
|
||
|
||
// 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) {
|
||
frontend->logging.log_compass = true;
|
||
|
||
// If the magnetometer has timed out (been rejected 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 (magTimeout && (maxCount > 1) && !onGround && imuSampleTime_ms - ekfStartTime_ms > 30000) {
|
||
|
||
// 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;
|
||
GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_WARNING, "EKF2 IMU%u switching to compass %u",(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
|
||
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;
|
||
}
|
||
}
|
||
}
|
||
|
||
// detect changes to magnetometer offset parameters and reset states
|
||
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(magSelectIndex);
|
||
bool changeDetected = lastMagOffsetsValid && (nowMagOffsets != lastMagOffsets);
|
||
if (changeDetected) {
|
||
// zero the learned magnetometer bias states
|
||
stateStruct.body_magfield.zero();
|
||
// clear the measurement buffer
|
||
storedMag.reset();
|
||
}
|
||
lastMagOffsets = nowMagOffsets;
|
||
lastMagOffsetsValid = true;
|
||
|
||
// store time of last measurement update
|
||
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec(magSelectIndex);
|
||
|
||
// estimate of time magnetometer measurement was taken, allowing for delays
|
||
magDataNew.time_ms = imuSampleTime_ms - frontend->magDelay_ms;
|
||
|
||
// Correct for the average intersampling delay due to the filter updaterate
|
||
magDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||
|
||
// read compass data and scale to improve numerical conditioning
|
||
magDataNew.mag = _ahrs->get_compass()->get_field(magSelectIndex) * 0.001f;
|
||
|
||
// check for consistent data between magnetometers
|
||
consistentMagData = _ahrs->get_compass()->consistent();
|
||
|
||
// save magnetometer measurement to buffer to be fused later
|
||
storedMag.push(magDataNew);
|
||
}
|
||
}
|
||
|
||
/********************************************************
|
||
* Inertial Measurements *
|
||
********************************************************/
|
||
|
||
/*
|
||
* Read IMU delta angle and delta velocity measurements and downsample to 100Hz
|
||
* for storage in the data buffers used by the EKF. If the IMU data arrives at
|
||
* lower rate than 100Hz, then no downsampling or upsampling will be performed.
|
||
* Downsampling is done using a method that does not introduce coning or sculling
|
||
* errors.
|
||
*/
|
||
void NavEKF2_core::readIMUData()
|
||
{
|
||
const AP_InertialSensor &ins = _ahrs->get_ins();
|
||
|
||
// average IMU sampling rate
|
||
dtIMUavg = ins.get_loop_delta_t();
|
||
|
||
// the imu sample time is used as a common time reference throughout the filter
|
||
imuSampleTime_ms = AP_HAL::millis();
|
||
|
||
// use the nominated imu or primary if not available
|
||
if (ins.use_accel(imu_index)) {
|
||
readDeltaVelocity(imu_index, imuDataNew.delVel, imuDataNew.delVelDT);
|
||
} else {
|
||
readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT);
|
||
}
|
||
|
||
// 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);
|
||
}
|
||
imuDataNew.delAngDT = MAX(ins.get_delta_angle_dt(imu_index),1.0e-4f);
|
||
|
||
// Get current time stamp
|
||
imuDataNew.time_ms = imuSampleTime_ms;
|
||
|
||
// 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
|
||
imuQuatDownSampleNew.rotate(imuDataNew.delAng);
|
||
imuQuatDownSampleNew.normalize();
|
||
|
||
// Rotate the latest delta velocity into body frame at the start of accumulation
|
||
Matrix3f deltaRotMat;
|
||
imuQuatDownSampleNew.rotation_matrix(deltaRotMat);
|
||
|
||
// Apply the delta velocity to the delta velocity accumulator
|
||
imuDataDownSampledNew.delVel += deltaRotMat*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 >= EKF_TARGET_DT && startPredictEnabled) || (dtIMUavg*(float)framesSincePredict >= 2.0f*EKF_TARGET_DT)) {
|
||
|
||
// 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
|
||
storedIMU.push_youngest_element(imuDataDownSampledNew);
|
||
|
||
// 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;
|
||
|
||
// 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;
|
||
|
||
// extract the oldest available data from the FIFO buffer
|
||
imuDataDelayed = storedIMU.pop_oldest_element();
|
||
|
||
// protect against delta time going to zero
|
||
// TODO - check if calculations can tolerate 0
|
||
float minDT = 0.1f*dtEkfAvg;
|
||
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
|
||
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);
|
||
|
||
// correct the extracted IMU data for sensor errors
|
||
delAngCorrected = imuDataDelayed.delAng;
|
||
delVelCorrected = imuDataDelayed.delVel;
|
||
correctDeltaAngle(delAngCorrected, imuDataDelayed.delAngDT);
|
||
correctDeltaVelocity(delVelCorrected, imuDataDelayed.delVelDT);
|
||
|
||
} else {
|
||
// we don't have new IMU data in the buffer so don't run filter updates on this time step
|
||
runUpdates = false;
|
||
}
|
||
}
|
||
|
||
// read the delta velocity and corresponding time interval from the IMU
|
||
// return false if data is not available
|
||
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);
|
||
dVel_dt = MAX(ins.get_delta_velocity_dt(ins_index),1.0e-4f);
|
||
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
|
||
// 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) {
|
||
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
|
||
gpsDataNew.time_ms = lastTimeGpsReceived_ms - frontend->_gpsDelay_ms;
|
||
|
||
// Correct for the average intersampling delay due to the filter updaterate
|
||
gpsDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||
|
||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||
gpsDataNew.time_ms = MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms);
|
||
|
||
// read the NED velocity from the GPS
|
||
gpsDataNew.vel = _ahrs->get_gps().velocity();
|
||
|
||
// 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
|
||
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 {
|
||
gpsSpdAccuracy = MAX(gpsSpdAccuracy,gpsSpdAccRaw);
|
||
gpsSpdAccuracy = MIN(gpsSpdAccuracy,50.0f);
|
||
}
|
||
gpsPosAccuracy *= (1.0f - alpha);
|
||
float gpsPosAccRaw;
|
||
if (!_ahrs->get_gps().horizontal_accuracy(gpsPosAccRaw)) {
|
||
gpsPosAccuracy = 0.0f;
|
||
} else {
|
||
gpsPosAccuracy = MAX(gpsPosAccuracy,gpsPosAccRaw);
|
||
gpsPosAccuracy = MIN(gpsPosAccuracy,100.0f);
|
||
}
|
||
gpsHgtAccuracy *= (1.0f - alpha);
|
||
float gpsHgtAccRaw;
|
||
if (!_ahrs->get_gps().vertical_accuracy(gpsHgtAccRaw)) {
|
||
gpsHgtAccuracy = 0.0f;
|
||
} else {
|
||
gpsHgtAccuracy = MAX(gpsHgtAccuracy,gpsHgtAccRaw);
|
||
gpsHgtAccuracy = MIN(gpsHgtAccuracy,100.0f);
|
||
}
|
||
|
||
// 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;
|
||
}
|
||
|
||
// Check if GPS can output vertical velocity and set GPS fusion mode accordingly
|
||
if (_ahrs->get_gps().have_vertical_velocity() && frontend->_fusionModeGPS == 0) {
|
||
useGpsVertVel = true;
|
||
} else {
|
||
useGpsVertVel = false;
|
||
}
|
||
|
||
// Monitor quality of the GPS velocity data before and after alignment using separate checks
|
||
if (PV_AidingMode != AID_ABSOLUTE) {
|
||
// Pre-alignment checks
|
||
gpsGoodToAlign = calcGpsGoodToAlign();
|
||
} else {
|
||
gpsGoodToAlign = false;
|
||
}
|
||
|
||
// Post-alignment checks
|
||
calcGpsGoodForFlight();
|
||
|
||
// Read the GPS locaton in WGS-84 lat,long,height coordinates
|
||
const struct Location &gpsloc = _ahrs->get_gps().location();
|
||
|
||
// Set the EKF origin and magnetic field declination if not previously set and GPS checks have passed
|
||
if (gpsGoodToAlign && !validOrigin) {
|
||
setOrigin();
|
||
|
||
// set the NE earth magnetic field states using the published declination
|
||
// and set the corresponding variances and covariances
|
||
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;
|
||
|
||
// Set the uncertinty of the GPS origin height
|
||
ekfOriginHgtVar = sq(gpsHgtAccuracy);
|
||
|
||
}
|
||
|
||
// convert GPS measurements to local NED and save to buffer to be fused later if we have a valid origin
|
||
if (validOrigin) {
|
||
gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
|
||
gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt);
|
||
storedGPS.push(gpsDataNew);
|
||
// declare GPS available for use
|
||
gpsNotAvailable = false;
|
||
}
|
||
|
||
frontend->logging.log_gps = true;
|
||
|
||
} else {
|
||
// report GPS fix status
|
||
gpsCheckStatus.bad_fix = true;
|
||
}
|
||
}
|
||
}
|
||
|
||
// read the delta angle and corresponding time interval from the IMU
|
||
// return false if data is not available
|
||
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);
|
||
frontend->logging.log_imu = true;
|
||
return true;
|
||
}
|
||
return false;
|
||
}
|
||
|
||
|
||
/********************************************************
|
||
* Height Measurements *
|
||
********************************************************/
|
||
|
||
// check for new pressure altitude measurement data and update stored measurement if available
|
||
void NavEKF2_core::readBaroData()
|
||
{
|
||
// check to see if baro measurement has changed so we know if a new measurement has arrived
|
||
// do not accept data at a faster rate than 14Hz to avoid overflowing the FIFO buffer
|
||
if (frontend->_baro.get_last_update() - lastBaroReceived_ms > 70) {
|
||
frontend->logging.log_baro = true;
|
||
|
||
baroDataNew.hgt = frontend->_baro.get_altitude();
|
||
|
||
// 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 (getTakeoffExpected()) {
|
||
baroDataNew.hgt = MAX(baroDataNew.hgt, meaHgtAtTakeOff);
|
||
}
|
||
|
||
// time stamp used to check for new measurement
|
||
lastBaroReceived_ms = frontend->_baro.get_last_update();
|
||
|
||
// estimate of time height measurement was taken, allowing for delays
|
||
baroDataNew.time_ms = lastBaroReceived_ms - frontend->_hgtDelay_ms;
|
||
|
||
// Correct for the average intersampling delay due to the filter updaterate
|
||
baroDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||
|
||
// Prevent time delay exceeding age of oldest IMU data in the buffer
|
||
baroDataNew.time_ms = MAX(baroDataNew.time_ms,imuDataDelayed.time_ms);
|
||
|
||
// save baro measurement to buffer to be fused later
|
||
storedBaro.push(baroDataNew);
|
||
}
|
||
}
|
||
|
||
// 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 from alternate height data source
|
||
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);
|
||
}
|
||
|
||
// calculate filtered offset between GPS height measurement and EKF height estimate
|
||
// offset should be subtracted from GPS measurement to match filter estimate
|
||
// offset is used to switch reversion to GPS from alternate height data source
|
||
void NavEKF2_core::calcFiltGpsHgtOffset()
|
||
{
|
||
// 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);
|
||
} else if (activeHgtSource == HGT_SOURCE_GPS) {
|
||
// by definition we are using GPS height as the EKF datum in this mode
|
||
// so cannot run this filter
|
||
return;
|
||
}
|
||
lastOriginHgtTime_ms = imuDataDelayed.time_ms;
|
||
|
||
// calculate the observation variance assuming EKF error relative to datum is independant of GPS observation error
|
||
// 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);
|
||
|
||
// correct the EKF origin and variance estimate if the innovation variance ratio is < 5-sigma
|
||
if (ratio < 5.0f) {
|
||
EKF_origin.alt -= (int)(100.0f * gain * innovation);
|
||
ekfOriginHgtVar -= fmaxf(gain * ekfOriginHgtVar , 0.0f);
|
||
}
|
||
}
|
||
|
||
/********************************************************
|
||
* 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();
|
||
tasDataNew.time_ms = timeTasReceived_ms - frontend->tasDelay_ms;
|
||
|
||
// Correct for the average intersampling delay due to the filter update rate
|
||
tasDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||
|
||
// Save data into the buffer to be fused when the fusion time horizon catches up with it
|
||
storedTAS.push(tasDataNew);
|
||
}
|
||
// 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);
|
||
}
|
||
|
||
#endif // HAL_CPU_CLASS
|