mirror of https://github.com/ArduPilot/ardupilot
804 lines
36 KiB
C++
804 lines
36 KiB
C++
#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
|
||
// don't allow range to go below 5cm because this can cause problems with optical flow processing
|
||
rngOnGnd = MAX(frontend->_rng.ground_clearance_cm_orient(ROTATION_PITCH_270) * 0.01f, 0.05f);
|
||
|
||
// 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.get_orientation(sensorIndex) == ROTATION_PITCH_270) && (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 {
|
||
minIndex = 0;
|
||
maxIndex = 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);
|
||
|
||
// get position in body frame for the current sensor
|
||
rangeDataNew.sensor_idx = sensorIndex;
|
||
|
||
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
|
||
storedRange.push(rangeDataNew);
|
||
|
||
// indicate we have updated the measurement
|
||
rngValidMeaTime_ms = imuSampleTime_ms;
|
||
|
||
} 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);
|
||
|
||
// indicate we have updated the measurement
|
||
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, const Vector3f &posOffset)
|
||
{
|
||
// 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;
|
||
}
|
||
// by definition if this function is called, then flow measurements have been provided so we
|
||
// need to run the optical flow takeoff detection
|
||
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 body rates for bias and write
|
||
ofDataNew.bodyRadXYZ.x = rawGyroRates.x - flowGyroBias.x;
|
||
ofDataNew.bodyRadXYZ.y = rawGyroRates.y - flowGyroBias.y;
|
||
// the sensor interface doesn't provide a z axis rate so use the rate from the nav sensor instead
|
||
if (delTimeOF > 0.001f) {
|
||
// first preference is to use the rate averaged over the same sampling period as the flow sensor
|
||
ofDataNew.bodyRadXYZ.z = delAngBodyOF.z / delTimeOF;
|
||
} else if (imuDataNew.delAngDT > 0.001f){
|
||
// second preference is to use most recent IMU data
|
||
ofDataNew.bodyRadXYZ.z = imuDataNew.delAng.z / imuDataNew.delAngDT;
|
||
} else {
|
||
// third preference is use zero
|
||
ofDataNew.bodyRadXYZ.z = 0.0f;
|
||
}
|
||
// write uncorrected flow rate measurements
|
||
// 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 the flow sensor position in body frame
|
||
ofDataNew.body_offset = &posOffset;
|
||
// write flow rate measurements corrected for body rates
|
||
ofDataNew.flowRadXYcomp.x = ofDataNew.flowRadXY.x + ofDataNew.bodyRadXYZ.x;
|
||
ofDataNew.flowRadXYcomp.y = ofDataNew.flowRadXY.y + ofDataNew.bodyRadXYZ.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_INFO, "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);
|
||
accelPosOffset = ins.get_imu_pos_offset(imu_index);
|
||
} else {
|
||
readDeltaVelocity(ins.get_primary_accel(), imuDataNew.delVel, imuDataNew.delVelDT);
|
||
accelPosOffset = ins.get_imu_pos_offset(ins.get_primary_accel());
|
||
}
|
||
|
||
// 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 the target EKF time step has been accumulated, and the frontend has allowed start of a new predict cycle,
|
||
* then store the accumulated IMU data to be used by the state prediction, ignoring the frontend permission if more
|
||
* than twice the target time has lapsed. Adjust the target EKF step time threshold to allow for timing jitter in the
|
||
* IMU data.
|
||
*/
|
||
if ((dtIMUavg*(float)framesSincePredict >= (EKF_TARGET_DT-(dtIMUavg*0.5)) &&
|
||
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);
|
||
|
||
updateTimingStatistics();
|
||
|
||
// 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);
|
||
|
||
// Get which GPS we are using for position information
|
||
gpsDataNew.sensor_idx = _ahrs->get_gps().primary_sensor();
|
||
|
||
// 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 independent 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);
|
||
}
|
||
|
||
/********************************************************
|
||
* Range Beacon Measurements *
|
||
********************************************************/
|
||
|
||
// check for new range beacon data and push to data buffer if available
|
||
void NavEKF2_core::readRngBcnData()
|
||
{
|
||
// get the location of the beacon data
|
||
const AP_Beacon *beacon = _ahrs->get_beacon();
|
||
|
||
// exit immediately if no beacon object
|
||
if (beacon == nullptr) {
|
||
return;
|
||
}
|
||
|
||
// get the number of beacons in use
|
||
N_beacons = beacon->count();
|
||
|
||
// search through all the beacons for new data and if we find it stop searching and push the data into the observation buffer
|
||
bool newDataToPush = false;
|
||
uint8_t numRngBcnsChecked = 0;
|
||
// start the search one index up from where we left it last time
|
||
uint8_t index = lastRngBcnChecked;
|
||
while (!newDataToPush && numRngBcnsChecked < N_beacons) {
|
||
// track the number of beacons checked
|
||
numRngBcnsChecked++;
|
||
|
||
// move to next beacon, wrap index if necessary
|
||
index++;
|
||
if (index >= N_beacons) {
|
||
index = 0;
|
||
}
|
||
|
||
// check that the beacon is healthy and has new data
|
||
if (beacon->beacon_healthy(index) &&
|
||
beacon->beacon_last_update_ms(index) != lastTimeRngBcn_ms[index])
|
||
{
|
||
// set the timestamp, correcting for measurement delay and average intersampling delay due to the filter update rate
|
||
lastTimeRngBcn_ms[index] = beacon->beacon_last_update_ms(index);
|
||
rngBcnDataNew.time_ms = lastTimeRngBcn_ms[index] - frontend->_rngBcnDelay_ms - localFilterTimeStep_ms/2;
|
||
|
||
// set the range noise
|
||
// TODO the range library should provide the noise/accuracy estimate for each beacon
|
||
rngBcnDataNew.rngErr = frontend->_rngBcnNoise;
|
||
|
||
// set the range measurement
|
||
rngBcnDataNew.rng = beacon->beacon_distance(index);
|
||
|
||
// set the beacon position
|
||
rngBcnDataNew.beacon_posNED = beacon->beacon_position(index);
|
||
|
||
// identify the beacon identifier
|
||
rngBcnDataNew.beacon_ID = index;
|
||
|
||
// indicate we have new data to push to the buffer
|
||
newDataToPush = true;
|
||
|
||
// update the last checked index
|
||
lastRngBcnChecked = index;
|
||
}
|
||
}
|
||
|
||
// Check if the beacon system has returned a 3D fix
|
||
if (beacon->get_vehicle_position_ned(beaconVehiclePosNED, beaconVehiclePosErr)) {
|
||
rngBcnLast3DmeasTime_ms = imuSampleTime_ms;
|
||
}
|
||
|
||
// Check if the range beacon data can be used to align the vehicle position
|
||
if (imuSampleTime_ms - rngBcnLast3DmeasTime_ms < 250 && beaconVehiclePosErr < 1.0f && rngBcnAlignmentCompleted) {
|
||
// check for consistency between the position reported by the beacon and the position from the 3-State alignment filter
|
||
float posDiffSq = sq(receiverPos.x - beaconVehiclePosNED.x) + sq(receiverPos.y - beaconVehiclePosNED.y);
|
||
float posDiffVar = sq(beaconVehiclePosErr) + receiverPosCov[0][0] + receiverPosCov[1][1];
|
||
if (posDiffSq < 9.0f*posDiffVar) {
|
||
rngBcnGoodToAlign = true;
|
||
// Set the EKF origin and magnetic field declination if not previously set
|
||
if (!validOrigin && PV_AidingMode != AID_ABSOLUTE) {
|
||
// get origin from beacon system
|
||
Location origin_loc;
|
||
if (beacon->get_origin(origin_loc)) {
|
||
setOriginLLH(origin_loc);
|
||
|
||
// set the NE earth magnetic field states using the published declination
|
||
// and set the corresponding variances and covariances
|
||
alignMagStateDeclination();
|
||
|
||
// Set the height of the NED origin to ‘height of baro height datum relative to GPS height datum'
|
||
EKF_origin.alt = origin_loc.alt - baroDataNew.hgt;
|
||
|
||
// Set the uncertainty of the origin height
|
||
ekfOriginHgtVar = sq(beaconVehiclePosErr);
|
||
}
|
||
}
|
||
} else {
|
||
rngBcnGoodToAlign = false;
|
||
}
|
||
} else {
|
||
rngBcnGoodToAlign = false;
|
||
}
|
||
|
||
// Save data into the buffer to be fused when the fusion time horizon catches up with it
|
||
if (newDataToPush) {
|
||
storedRangeBeacon.push(rngBcnDataNew);
|
||
}
|
||
|
||
// Check the buffer for measurements that have been overtaken by the fusion time horizon and need to be fused
|
||
rngBcnDataToFuse = storedRangeBeacon.recall(rngBcnDataDelayed,imuDataDelayed.time_ms);
|
||
|
||
}
|
||
|
||
/*
|
||
update timing statistics structure
|
||
*/
|
||
void NavEKF2_core::updateTimingStatistics(void)
|
||
{
|
||
if (timing.count == 0) {
|
||
timing.dtIMUavg_max = dtIMUavg;
|
||
timing.dtIMUavg_min = dtIMUavg;
|
||
timing.dtEKFavg_max = dtEkfAvg;
|
||
timing.dtEKFavg_min = dtEkfAvg;
|
||
timing.delAngDT_max = imuDataDelayed.delAngDT;
|
||
timing.delAngDT_min = imuDataDelayed.delAngDT;
|
||
timing.delVelDT_max = imuDataDelayed.delVelDT;
|
||
timing.delVelDT_min = imuDataDelayed.delVelDT;
|
||
} else {
|
||
timing.dtIMUavg_max = MAX(timing.dtIMUavg_max, dtIMUavg);
|
||
timing.dtIMUavg_min = MIN(timing.dtIMUavg_min, dtIMUavg);
|
||
timing.dtEKFavg_max = MAX(timing.dtEKFavg_max, dtEkfAvg);
|
||
timing.dtEKFavg_min = MIN(timing.dtEKFavg_min, dtEkfAvg);
|
||
timing.delAngDT_max = MAX(timing.delAngDT_max, imuDataDelayed.delAngDT);
|
||
timing.delAngDT_min = MIN(timing.delAngDT_min, imuDataDelayed.delAngDT);
|
||
timing.delVelDT_max = MAX(timing.delVelDT_max, imuDataDelayed.delVelDT);
|
||
timing.delVelDT_min = MIN(timing.delVelDT_min, imuDataDelayed.delVelDT);
|
||
}
|
||
timing.count++;
|
||
}
|
||
|
||
// get timing statistics structure
|
||
void NavEKF2_core::getTimingStatistics(struct ekf_timing &_timing)
|
||
{
|
||
_timing = timing;
|
||
memset(&timing, 0, sizeof(timing));
|
||
}
|
||
|
||
#endif // HAL_CPU_CLASS
|