mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-04 23:18:28 -04:00
f539b597a3
Use the more robust, but less accurate compass heading fusion up to 5m altitude Wait for the magnetometer data fusion time offset to be correct before using data to reset states Don't reset magnetic field states if the vehicle is rotating rapidly as timing offsets will produce large errors When doing the yaw angle reset, apply the reset increment to all quaternions stored in the output buffer to avoid transients produced by yaw rotations and the 0.25 second fusion time horizon offset. Only do the one yaw and mag reset at 5m, not two at 1.5 and 5.0m Always re-do the yaw and mag reset when leaving the ground.
789 lines
34 KiB
C++
789 lines
34 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 <stdio.h>
|
||
|
||
extern const AP_HAL::HAL& hal;
|
||
|
||
|
||
/********************************************************
|
||
* OPT FLOW AND RANGE FINDER *
|
||
********************************************************/
|
||
|
||
// Read the range finder and take new measurements if available
|
||
// Read at 20Hz and 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 = _rng.ground_clearance_cm() * 0.01f;
|
||
if (_rng.status() == RangeFinder::RangeFinder_Good && (imuSampleTime_ms - lastRngMeasTime_ms) > 50) {
|
||
// store samples and sample time into a ring buffer
|
||
rngMeasIndex ++;
|
||
if (rngMeasIndex > 2) {
|
||
rngMeasIndex = 0;
|
||
}
|
||
storedRngMeasTime_ms[rngMeasIndex] = imuSampleTime_ms;
|
||
storedRngMeas[rngMeasIndex] = _rng.distance_cm() * 0.01f;
|
||
// check for three fresh samples and take median
|
||
bool sampleFresh[3];
|
||
for (uint8_t index = 0; index <= 2; index++) {
|
||
sampleFresh[index] = (imuSampleTime_ms - storedRngMeasTime_ms[index]) < 500;
|
||
}
|
||
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;
|
||
}
|
||
rngMea = max(storedRngMeas[midIndex],rngOnGnd);
|
||
newDataRng = true;
|
||
rngValidMeaTime_ms = imuSampleTime_ms;
|
||
} else if (onGround) {
|
||
// if on ground and no return, we assume on ground range
|
||
rngMea = rngOnGnd;
|
||
newDataRng = true;
|
||
rngValidMeaTime_ms = imuSampleTime_ms;
|
||
} else {
|
||
newDataRng = false;
|
||
}
|
||
lastRngMeasTime_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);
|
||
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
|
||
ofDataNew.time_ms = imuSampleTime_ms - frontend._flowDelay_ms - frontend.flowTimeDeltaAvg_ms/2;
|
||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||
ofDataNew.time_ms = roundToNearest(ofDataNew.time_ms, frontend.fusionTimeStep_ms);
|
||
// 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
|
||
StoreOF();
|
||
// Check for data at the fusion time horizon
|
||
newDataFlow = RecallOF();
|
||
}
|
||
}
|
||
|
||
// store OF data in a history array
|
||
void NavEKF2_core::StoreOF()
|
||
{
|
||
if (ofStoreIndex >= OBS_BUFFER_LENGTH) {
|
||
ofStoreIndex = 0;
|
||
}
|
||
storedOF[ofStoreIndex] = ofDataNew;
|
||
ofStoreIndex += 1;
|
||
}
|
||
|
||
// return newest un-used optical flow data that has fallen behind the fusion time horizon
|
||
// if no un-used data is available behind the fusion horizon, return false
|
||
bool NavEKF2_core::RecallOF()
|
||
{
|
||
of_elements dataTemp;
|
||
of_elements dataTempZero;
|
||
dataTempZero.time_ms = 0;
|
||
uint32_t temp_ms = 0;
|
||
uint8_t bestIndex = 0;
|
||
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
|
||
dataTemp = storedOF[i];
|
||
// find a measurement older than the fusion time horizon that we haven't checked before
|
||
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
|
||
// Find the most recent non-stale measurement that meets the time horizon criteria
|
||
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
|
||
ofDataDelayed = dataTemp;
|
||
temp_ms = dataTemp.time_ms;
|
||
bestIndex = i;
|
||
}
|
||
}
|
||
}
|
||
if (temp_ms != 0) {
|
||
// zero the time stamp for that piece of data so we won't use it again
|
||
storedOF[bestIndex]=dataTempZero;
|
||
return true;
|
||
} else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
/********************************************************
|
||
* 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
|
||
if (firstMagYawInit && (frontend._magCal != 2) && _ahrs->get_compass()->healthy(0)) {
|
||
magOffsets = _ahrs->get_compass()->get_offsets(0) - stateStruct.body_magfield*1000.0f;
|
||
return true;
|
||
} else {
|
||
magOffsets = _ahrs->get_compass()->get_offsets(0);
|
||
return false;
|
||
}
|
||
}
|
||
|
||
// check for new magnetometer data and update store measurements if available
|
||
void NavEKF2_core::readMagData()
|
||
{
|
||
// 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) {
|
||
// store time of last measurement update
|
||
lastMagUpdate_us = _ahrs->get_compass()->last_update_usec();
|
||
|
||
// estimate of time magnetometer measurement was taken, allowing for delays
|
||
magDataNew.time_ms = imuSampleTime_ms - frontend.magDelay_ms;
|
||
|
||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||
magDataNew.time_ms = roundToNearest(magDataNew.time_ms, frontend.fusionTimeStep_ms);
|
||
|
||
// read compass data and scale to improve numerical conditioning
|
||
magDataNew.mag = _ahrs->get_compass()->get_field() * 0.001f;
|
||
|
||
// check for consistent data between magnetometers
|
||
consistentMagData = _ahrs->get_compass()->consistent();
|
||
|
||
// check if compass offsets have been changed and adjust EKF bias states to maintain consistent innovations
|
||
if (_ahrs->get_compass()->healthy(0)) {
|
||
Vector3f nowMagOffsets = _ahrs->get_compass()->get_offsets(0);
|
||
bool changeDetected = (!is_equal(nowMagOffsets.x,lastMagOffsets.x) || !is_equal(nowMagOffsets.y,lastMagOffsets.y) || !is_equal(nowMagOffsets.z,lastMagOffsets.z));
|
||
// Ignore bias changes before final mag field and yaw initialisation, as there may have been a compass calibration
|
||
if (changeDetected && firstMagYawInit) {
|
||
stateStruct.body_magfield.x += (nowMagOffsets.x - lastMagOffsets.x) * 0.001f;
|
||
stateStruct.body_magfield.y += (nowMagOffsets.y - lastMagOffsets.y) * 0.001f;
|
||
stateStruct.body_magfield.z += (nowMagOffsets.z - lastMagOffsets.z) * 0.001f;
|
||
}
|
||
lastMagOffsets = nowMagOffsets;
|
||
}
|
||
|
||
// save magnetometer measurement to buffer to be fused later
|
||
StoreMag();
|
||
}
|
||
}
|
||
// store magnetometer data in a history array
|
||
void NavEKF2_core::StoreMag()
|
||
{
|
||
if (magStoreIndex >= OBS_BUFFER_LENGTH) {
|
||
magStoreIndex = 0;
|
||
}
|
||
storedMag[magStoreIndex] = magDataNew;
|
||
magStoreIndex += 1;
|
||
}
|
||
|
||
// return newest un-used magnetometer data that has fallen behind the fusion time horizon
|
||
// if no un-used data is available behind the fusion horizon, return false
|
||
bool NavEKF2_core::RecallMag()
|
||
{
|
||
mag_elements dataTemp;
|
||
mag_elements dataTempZero;
|
||
dataTempZero.time_ms = 0;
|
||
uint32_t temp_ms = 0;
|
||
uint8_t bestIndex = 0;
|
||
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
|
||
dataTemp = storedMag[i];
|
||
// find a measurement older than the fusion time horizon that we haven't checked before
|
||
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
|
||
// Find the most recent non-stale measurement that meets the time horizon criteria
|
||
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
|
||
magDataDelayed = dataTemp;
|
||
temp_ms = dataTemp.time_ms;
|
||
bestIndex = i;
|
||
}
|
||
}
|
||
}
|
||
if (temp_ms != 0) {
|
||
// zero the time stamp for that piece of data so we won't use it again
|
||
storedMag[bestIndex]=dataTempZero;
|
||
return true;
|
||
} else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
|
||
/********************************************************
|
||
* Inertial Measurements *
|
||
********************************************************/
|
||
|
||
// update IMU delta angle and delta velocity measurements
|
||
void NavEKF2_core::readIMUData()
|
||
{
|
||
const AP_InertialSensor &ins = _ahrs->get_ins();
|
||
|
||
// average IMU sampling rate
|
||
dtIMUavg = 1.0f/ins.get_sample_rate();
|
||
|
||
// the imu sample time is used as a common time reference throughout the filter
|
||
imuSampleTime_ms = hal.scheduler->millis();
|
||
|
||
if (ins.use_accel(0) && ins.use_accel(1)) {
|
||
// dual accel mode
|
||
// delta time from each IMU
|
||
float dtDelVel0 = dtIMUavg;
|
||
float dtDelVel1 = dtIMUavg;
|
||
// delta velocity vector from each IMU
|
||
Vector3f delVel0, delVel1;
|
||
|
||
// Get delta velocity and time data from each IMU
|
||
readDeltaVelocity(0, delVel0, dtDelVel0);
|
||
readDeltaVelocity(1, delVel1, dtDelVel1);
|
||
|
||
// apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU 0
|
||
float alpha = 1.0f - 5.0f*dtDelVel0;
|
||
imuNoiseFiltState0 = maxf(ins.get_vibration_levels(0).length(), alpha*imuNoiseFiltState0);
|
||
|
||
// apply a peak hold 0.2 second time constant decaying envelope filter to the noise length on IMU 1
|
||
alpha = 1.0f - 5.0f*dtDelVel1;
|
||
imuNoiseFiltState1 = maxf(ins.get_vibration_levels(1).length(), alpha*imuNoiseFiltState1);
|
||
|
||
// calculate the filtered difference between acceleration vectors from IMU 0 and 1
|
||
// apply a LPF filter with a 1.0 second time constant
|
||
alpha = constrain_float(0.5f*(dtDelVel0 + dtDelVel1),0.0f,1.0f);
|
||
accelDiffFilt = (ins.get_accel(0) - ins.get_accel(1)) * alpha + accelDiffFilt * (1.0f - alpha);
|
||
float accelDiffLength = accelDiffFilt.length();
|
||
|
||
// Check the difference for excessive error and use the IMU with less noise
|
||
// Apply hysteresis to prevent rapid switching
|
||
if (accelDiffLength > 1.8f || (accelDiffLength > 1.2f && lastImuSwitchState != IMUSWITCH_MIXED)) {
|
||
if (lastImuSwitchState == IMUSWITCH_MIXED) {
|
||
// no previous fail so switch to the IMU with least noise
|
||
if (imuNoiseFiltState0 < imuNoiseFiltState1) {
|
||
lastImuSwitchState = IMUSWITCH_IMU0;
|
||
// Get data from IMU 0
|
||
imuDataNew.delVel = delVel0;
|
||
imuDataNew.delVelDT = dtDelVel0;
|
||
} else {
|
||
lastImuSwitchState = IMUSWITCH_IMU1;
|
||
// Get data from IMU 1
|
||
imuDataNew.delVel = delVel1;
|
||
imuDataNew.delVelDT = dtDelVel1;
|
||
}
|
||
} else if (lastImuSwitchState == IMUSWITCH_IMU0) {
|
||
// IMU 1 previously failed so require 5 m/s/s less noise on IMU 1 to switch
|
||
if (imuNoiseFiltState0 - imuNoiseFiltState1 > 5.0f) {
|
||
// IMU 1 is significantly less noisy, so switch
|
||
lastImuSwitchState = IMUSWITCH_IMU1;
|
||
// Get data from IMU 1
|
||
imuDataNew.delVel = delVel1;
|
||
imuDataNew.delVelDT = dtDelVel1;
|
||
}
|
||
} else {
|
||
// IMU 0 previously failed so require 5 m/s/s less noise on IMU 0 to switch across
|
||
if (imuNoiseFiltState1 - imuNoiseFiltState0 > 5.0f) {
|
||
// IMU 0 is significantly less noisy, so switch
|
||
lastImuSwitchState = IMUSWITCH_IMU0;
|
||
// Get data from IMU 0
|
||
imuDataNew.delVel = delVel0;
|
||
imuDataNew.delVelDT = dtDelVel0;
|
||
}
|
||
}
|
||
} else {
|
||
lastImuSwitchState = IMUSWITCH_MIXED;
|
||
// Use a blend of both accelerometers
|
||
imuDataNew.delVel = (delVel0 + delVel1)*0.5f;
|
||
imuDataNew.delVelDT = (dtDelVel0 + dtDelVel1)*0.5f;
|
||
}
|
||
} else {
|
||
// single accel mode - one of the first two accelerometers are unhealthy, not available or de-selected by the user
|
||
// set the switch state based on the IMU we are using to make the data source selection visible
|
||
if (ins.use_accel(0)) {
|
||
readDeltaVelocity(0, imuDataNew.delVel, imuDataNew.delVelDT);
|
||
lastImuSwitchState = IMUSWITCH_IMU0;
|
||
} else if (ins.use_accel(1)) {
|
||
readDeltaVelocity(1, imuDataNew.delVel, imuDataNew.delVelDT);
|
||
lastImuSwitchState = IMUSWITCH_IMU1;
|
||
} else {
|
||
readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT);
|
||
switch (ins.get_primary_accel()) {
|
||
case 0:
|
||
lastImuSwitchState = IMUSWITCH_IMU0;
|
||
break;
|
||
case 1:
|
||
lastImuSwitchState = IMUSWITCH_IMU1;
|
||
break;
|
||
default:
|
||
// we must be using an IMU which can't be properly represented so we set to "mixed"
|
||
lastImuSwitchState = IMUSWITCH_MIXED;
|
||
break;
|
||
}
|
||
}
|
||
}
|
||
|
||
// Get delta angle data from promary gyro
|
||
readDeltaAngle(ins.get_primary_gyro(), imuDataNew.delAng);
|
||
imuDataNew.delAngDT = max(ins.get_delta_time(),1.0e-4f);
|
||
|
||
// get current time stamp
|
||
imuDataNew.time_ms = imuSampleTime_ms;
|
||
|
||
// save data in the FIFO buffer
|
||
StoreIMU();
|
||
|
||
// extract the oldest available data from the FIFO buffer
|
||
imuDataDelayed = storedIMU[fifoIndexDelayed];
|
||
|
||
}
|
||
|
||
// store imu in the FIFO
|
||
void NavEKF2_core::StoreIMU()
|
||
{
|
||
// increment the index and write new data
|
||
fifoIndexNow = fifoIndexNow + 1;
|
||
if (fifoIndexNow >= IMU_BUFFER_LENGTH) {
|
||
fifoIndexNow = 0;
|
||
}
|
||
storedIMU[fifoIndexNow] = imuDataNew;
|
||
// set the index required to access the oldest data
|
||
fifoIndexDelayed = fifoIndexNow + 1;
|
||
if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
|
||
fifoIndexDelayed = 0;
|
||
}
|
||
}
|
||
|
||
// reset the stored imu history and store the current value
|
||
void NavEKF2_core::StoreIMU_reset()
|
||
{
|
||
// write current measurement to entire table
|
||
for (uint8_t i=0; i<IMU_BUFFER_LENGTH; i++) {
|
||
storedIMU[i] = imuDataNew;
|
||
}
|
||
imuDataDelayed = imuDataNew;
|
||
fifoIndexDelayed = fifoIndexNow+1;
|
||
if (fifoIndexDelayed >= IMU_BUFFER_LENGTH) {
|
||
fifoIndexDelayed = 0;
|
||
}
|
||
}
|
||
|
||
// recall IMU data from the FIFO
|
||
void NavEKF2_core::RecallIMU()
|
||
{
|
||
imuDataDelayed = storedIMU[fifoIndexDelayed];
|
||
}
|
||
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;
|
||
|
||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||
gpsDataNew.time_ms = roundToNearest(gpsDataNew.time_ms, frontend.fusionTimeStep_ms);
|
||
|
||
// 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 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 {
|
||
gpsSpdAccuracy = max(gpsSpdAccuracy,gpsSpdAccRaw);
|
||
}
|
||
|
||
// 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 {
|
||
// Post-alignment checks
|
||
calcGpsGoodForFlight();
|
||
}
|
||
|
||
// read latitutde and longitude from GPS and convert to local NE position relative to the stored origin
|
||
// If we don't have an origin, then set it to the current GPS coordinates
|
||
const struct Location &gpsloc = _ahrs->get_gps().location();
|
||
if (validOrigin) {
|
||
gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
|
||
} else if (gpsGoodToAlign) {
|
||
// Set the NE origin to the current GPS position
|
||
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;
|
||
// We are by definition at the origin at the instant of alignment so set NE position to zero
|
||
gpsDataNew.pos.zero();
|
||
// If GPS useage isn't explicitly prohibited, we switch to absolute position mode
|
||
if (isAiding && frontend._fusionModeGPS != 3) {
|
||
PV_AidingMode = AID_ABSOLUTE;
|
||
// Initialise EKF position and velocity states
|
||
ResetPosition();
|
||
ResetVelocity();
|
||
}
|
||
}
|
||
|
||
// save measurement to buffer to be fused later
|
||
StoreGPS();
|
||
|
||
// declare GPS available for use
|
||
gpsNotAvailable = false;
|
||
} else {
|
||
// report GPS fix status
|
||
gpsCheckStatus.bad_fix = true;
|
||
}
|
||
}
|
||
|
||
// 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
|
||
uint16_t gpsRetryTimeout_ms = useAirspeed() ? frontend.gpsRetryTimeUseTAS_ms : frontend.gpsRetryTimeNoTAS_ms;
|
||
|
||
// Set the time that copters will fly without a GPS lock before failing the GPS and switching to a non GPS mode
|
||
uint16_t gpsFailTimeout_ms = optFlowBackupAvailable ? frontend.gpsFailTimeWithFlow_ms : gpsRetryTimeout_ms;
|
||
|
||
// 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) {
|
||
|
||
// Let other processes know that GPS is not available and that a timeout has occurred
|
||
posTimeout = true;
|
||
velTimeout = true;
|
||
gpsNotAvailable = true;
|
||
|
||
// 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
|
||
frontend._fusionModeGPS = 3;
|
||
PV_AidingMode = AID_RELATIVE;
|
||
} else {
|
||
// store the current position
|
||
lastKnownPositionNE.x = stateStruct.position.x;
|
||
lastKnownPositionNE.y = stateStruct.position.y;
|
||
|
||
// put the filter into constant position mode
|
||
PV_AidingMode = AID_NONE;
|
||
|
||
// Reset the velocity and position states
|
||
ResetVelocity();
|
||
ResetPosition();
|
||
|
||
// Reset the normalised innovation to avoid false failing bad fusion tests
|
||
velTestRatio = 0.0f;
|
||
posTestRatio = 0.0f;
|
||
}
|
||
}
|
||
}
|
||
}
|
||
|
||
|
||
// store GPS data in a history array
|
||
void NavEKF2_core::StoreGPS()
|
||
{
|
||
if (gpsStoreIndex >= OBS_BUFFER_LENGTH) {
|
||
gpsStoreIndex = 0;
|
||
}
|
||
storedGPS[gpsStoreIndex] = gpsDataNew;
|
||
gpsStoreIndex += 1;
|
||
}
|
||
|
||
// return newest un-used GPS data that has fallen behind the fusion time horizon
|
||
// if no un-used data is available behind the fusion horizon, return false
|
||
bool NavEKF2_core::RecallGPS()
|
||
{
|
||
gps_elements dataTemp;
|
||
gps_elements dataTempZero;
|
||
dataTempZero.time_ms = 0;
|
||
uint32_t temp_ms = 0;
|
||
uint8_t bestIndex;
|
||
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
|
||
dataTemp = storedGPS[i];
|
||
// find a measurement older than the fusion time horizon that we haven't checked before
|
||
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
|
||
// Find the most recent non-stale measurement that meets the time horizon criteria
|
||
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
|
||
gpsDataDelayed = dataTemp;
|
||
temp_ms = dataTemp.time_ms;
|
||
bestIndex = i;
|
||
}
|
||
}
|
||
}
|
||
if (temp_ms != 0) {
|
||
// zero the time stamp for that piece of data so we won't use it again
|
||
storedGPS[bestIndex]=dataTempZero;
|
||
return true;
|
||
} else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
|
||
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 *
|
||
********************************************************/
|
||
|
||
// check for new altitude measurement data and update stored measurement if available
|
||
void NavEKF2_core::readHgtData()
|
||
{
|
||
// 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 (_baro.get_last_update() - lastHgtReceived_ms > 70) {
|
||
// Don't use Baro height if operating in optical flow mode as we use range finder instead
|
||
if (frontend._fusionModeGPS == 3 && frontend._altSource == 1) {
|
||
if ((imuSampleTime_ms - rngValidMeaTime_ms) < 2000) {
|
||
// adjust range finder measurement to allow for effect of vehicle tilt and height of sensor
|
||
baroDataNew.hgt = max(rngMea * Tnb_flow.c.z, rngOnGnd);
|
||
// calculate offset to baro data that enables baro to be used as a backup
|
||
// filter offset to reduce effect of baro noise and other transient errors on estimate
|
||
baroHgtOffset = 0.1f * (_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset;
|
||
} else if (isAiding && takeOffDetected) {
|
||
// we have lost range finder measurements and are in optical flow flight
|
||
// use baro measurement and correct for baro offset - failsafe use only as baro will drift
|
||
baroDataNew.hgt = max(_baro.get_altitude() - baroHgtOffset, rngOnGnd);
|
||
} else {
|
||
// If we are on ground and have no range finder reading, assume the nominal on-ground height
|
||
baroDataNew.hgt = rngOnGnd;
|
||
// calculate offset to baro data that enables baro to be used as a backup
|
||
// filter offset to reduce effect of baro noise and other transient errors on estimate
|
||
baroHgtOffset = 0.1f * (_baro.get_altitude() + stateStruct.position.z) + 0.9f * baroHgtOffset;
|
||
}
|
||
} else {
|
||
// Normal operation is to use baro measurement
|
||
baroDataNew.hgt = _baro.get_altitude();
|
||
}
|
||
|
||
// filtered baro data used to provide a reference for takeoff
|
||
// it is is reset to last height measurement on disarming in performArmingChecks()
|
||
if (!getTakeoffExpected()) {
|
||
const float gndHgtFiltTC = 0.5f;
|
||
const float dtBaro = frontend.hgtAvg_ms*1.0e-3f;
|
||
float alpha = constrain_float(dtBaro / (dtBaro+gndHgtFiltTC),0.0f,1.0f);
|
||
meaHgtAtTakeOff += (baroDataDelayed.hgt-meaHgtAtTakeOff)*alpha;
|
||
} else if (isAiding && getTakeoffExpected()) {
|
||
// 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
|
||
baroDataNew.hgt = max(baroDataNew.hgt, meaHgtAtTakeOff);
|
||
}
|
||
|
||
// time stamp used to check for new measurement
|
||
lastHgtReceived_ms = _baro.get_last_update();
|
||
|
||
// estimate of time height measurement was taken, allowing for delays
|
||
baroDataNew.time_ms = lastHgtReceived_ms - frontend._hgtDelay_ms;
|
||
|
||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||
baroDataNew.time_ms = roundToNearest(baroDataNew.time_ms, frontend.fusionTimeStep_ms);
|
||
|
||
// 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
|
||
StoreBaro();
|
||
}
|
||
}
|
||
|
||
// store baro in a history array
|
||
void NavEKF2_core::StoreBaro()
|
||
{
|
||
if (baroStoreIndex >= OBS_BUFFER_LENGTH) {
|
||
baroStoreIndex = 0;
|
||
}
|
||
storedBaro[baroStoreIndex] = baroDataNew;
|
||
baroStoreIndex += 1;
|
||
}
|
||
|
||
// return newest un-used baro data that has fallen behind the fusion time horizon
|
||
// if no un-used data is available behind the fusion horizon, return false
|
||
bool NavEKF2_core::RecallBaro()
|
||
{
|
||
baro_elements dataTemp;
|
||
baro_elements dataTempZero;
|
||
dataTempZero.time_ms = 0;
|
||
uint32_t temp_ms = 0;
|
||
uint8_t bestIndex = 0;
|
||
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
|
||
dataTemp = storedBaro[i];
|
||
// find a measurement older than the fusion time horizon that we haven't checked before
|
||
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
|
||
// Find the most recent non-stale measurement that meets the time horizon criteria
|
||
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
|
||
baroDataDelayed = dataTemp;
|
||
temp_ms = dataTemp.time_ms;
|
||
bestIndex = i;
|
||
}
|
||
}
|
||
}
|
||
if (temp_ms != 0) {
|
||
// zero the time stamp for that piece of data so we won't use it again
|
||
storedBaro[bestIndex]=dataTempZero;
|
||
return true;
|
||
} else {
|
||
return false;
|
||
}
|
||
}
|
||
|
||
|
||
|
||
/********************************************************
|
||
* 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;
|
||
// Assign measurement to nearest fusion interval so that multiple measurements can be fused on the same frame
|
||
// This allows us to perform the covariance prediction over longer time steps which reduces numerical precision errors
|
||
tasDataNew.time_ms = roundToNearest(tasDataNew.time_ms, frontend.fusionTimeStep_ms);
|
||
newDataTas = true;
|
||
StoreTAS();
|
||
RecallTAS();
|
||
} else {
|
||
newDataTas = false;
|
||
}
|
||
}
|
||
|
||
// Round to the nearest multiple of a integer
|
||
uint32_t NavEKF2_core::roundToNearest(uint32_t dividend, uint32_t divisor )
|
||
{
|
||
return ((uint32_t)round((float)dividend/float(divisor)))*divisor;
|
||
}
|
||
|
||
#endif // HAL_CPU_CLASS
|