ardupilot/libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp

831 lines
38 KiB
C++
Raw Normal View History

AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
#include <AP_HAL/AP_HAL.h>
#if HAL_CPU_CLASS >= HAL_CPU_CLASS_150
#include "AP_NavEKF3.h"
#include "AP_NavEKF3_core.h"
#include <AP_AHRS/AP_AHRS.h>
#include <AP_Vehicle/AP_Vehicle.h>
#include <GCS_MAVLink/GCS.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 NavEKF3_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);
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
// limit update rate to maximum allowed by data buffers
if ((imuSampleTime_ms - lastRngMeasTime_ms) > frontend->sensorIntervalMin_ms) {
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
// 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)) {
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
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;
}
}
}
}
void NavEKF3_core::writeBodyFrameOdom(float quality, const Vector3f &delPos, const Vector3f &delAng, float delTime, uint32_t timeStamp_ms, const Vector3f &posOffset)
{
// limit update rate to maximum allowed by sensor buffers and fusion process
// don't try to write to buffer until the filter has been initialised
if (((timeStamp_ms - bodyOdmMeasTime_ms) < frontend->sensorIntervalMin_ms) || (delTime < dtEkfAvg) || !statesInitialised) {
return;
}
bodyOdmDataNew.body_offset = &posOffset;
bodyOdmDataNew.vel = delPos * (1.0f/delTime);
bodyOdmDataNew.time_ms = timeStamp_ms;
bodyOdmDataNew.angRate = delAng * (1.0f/delTime);
bodyOdmMeasTime_ms = timeStamp_ms;
// simple model of accuracy
// TODO move this calculation outside of EKF into the sensor driver
const float minVelErr = 0.5f;
const float maxVelErr = 10.0f;
bodyOdmDataNew.velErr = minVelErr + (maxVelErr - minVelErr) * (1.0f - 0.01f * quality);
storedBodyOdm.push(bodyOdmDataNew);
}
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
// write the raw optical flow measurements
// this needs to be called externally.
void NavEKF3_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRates, Vector2f &rawGyroRates, uint32_t &msecFlowMeas, const Vector3f &posOffset)
{
// limit update rate to maximum allowed by sensor buffers
if ((imuSampleTime_ms - flowMeaTime_ms) < frontend->sensorIntervalMin_ms) {
return;
}
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
// The raw measurements need to be optical flow rates in radians/second averaged across the time since the last update
// The PX4Flow sensor outputs flow rates with the following axis and sign conventions:
// A positive X rate is produced by a positive sensor rotation about the X axis
// A positive Y rate is produced by a positive sensor rotation about the Y axis
// This filter uses a different definition of optical flow rates to the sensor with a positive optical flow rate produced by a
// negative rotation about that axis. For example a positive rotation of the flight vehicle about its X (roll) axis would produce a negative X flow rate
flowMeaTime_ms = imuSampleTime_ms;
// calculate bias errors on flow sensor gyro rates, but protect against spikes in data
// reset the accumulated body delta angle and time
// don't do the calculation if not enough time lapsed for a reliable body rate measurement
if (delTimeOF > 0.01f) {
flowGyroBias.x = 0.99f * flowGyroBias.x + 0.01f * constrain_float((rawGyroRates.x - delAngBodyOF.x/delTimeOF),-0.1f,0.1f);
flowGyroBias.y = 0.99f * flowGyroBias.y + 0.01f * constrain_float((rawGyroRates.y - delAngBodyOF.y/delTimeOF),-0.1f,0.1f);
delAngBodyOF.zero();
delTimeOF = 0.0f;
}
// 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);
}
}
/********************************************************
* MAGNETOMETER *
********************************************************/
// check for new magnetometer data and update store measurements if available
void NavEKF3_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;
}
// limit compass update rate 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) > 1000 * frontend->sensorIntervalMin_ms)) {
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
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, "EKF3 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 NavEKF3_core::readIMUData()
{
const AP_InertialSensor &ins = _ahrs->get_ins();
// calculate an averaged IMU update rate using a spike and lowpass filter combination
dtIMUavg = 0.02f * constrain_float(ins.get_loop_delta_t(),0.5f * dtIMUavg, 2.0f * dtIMUavg) + 0.98f * dtIMUavg;
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
// the imu sample time is used as a common time reference throughout the filter
imuSampleTime_ms = frontend->imuSampleTime_us / 1000;
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
// 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
2017-05-01 00:50:44 -03:00
* than twice the target time has lapsed. Adjust the target EKF step time threshold to allow for timing jitter in the
* IMU data.
*/
if ((imuDataDownSampledNew.delAngDT >= (EKF_TARGET_DT-(dtIMUavg*0.5)) && startPredictEnabled) ||
(imuDataDownSampledNew.delAngDT >= 2.0f*EKF_TARGET_DT)) {
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
// 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 using a combination spike and LPF
float dtNow = constrain_float(0.5f*(imuDataDownSampledNew.delAngDT+imuDataDownSampledNew.delVelDT),0.5f * dtEkfAvg, 2.0f * dtEkfAvg);
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
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
float minDT = 0.1f * dtEkfAvg;
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
imuDataDelayed.delAngDT = MAX(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = MAX(imuDataDelayed.delVelDT,minDT);
updateTimingStatistics();
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
// 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 NavEKF3_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 NavEKF3_core::readGpsData()
{
// check for new GPS data
// limit update rate to avoid overflowing the FIFO buffer
if (_ahrs->get_gps().last_message_time_ms() - lastTimeGpsReceived_ms > frontend->sensorIntervalMin_ms) {
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
if (_ahrs->get_gps().status() >= AP_GPS::GPS_OK_FIX_3D) {
// report GPS fix status
gpsCheckStatus.bad_fix = false;
// store fix time from previous read
secondLastGpsTime_ms = lastTimeGpsReceived_ms;
// get current fix time
lastTimeGpsReceived_ms = _ahrs->get_gps().last_message_time_ms();
// estimate when the GPS fix was valid, allowing for GPS processing and other delays
// ideally we should be using a timing signal from the GPS receiver to set this time
// Use the driver specified delay
gpsDataNew.time_ms = lastTimeGpsReceived_ms - (uint32_t)(_ahrs->get_gps().get_lag() * 1000.0f);
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
// Correct for the average intersampling delay due to the filter updaterate
gpsDataNew.time_ms -= localFilterTimeStep_ms/2;
// Prevent the time stamp falling outside the oldest and newest IMU data in the buffer
gpsDataNew.time_ms = MIN(MAX(gpsDataNew.time_ms,imuDataDelayed.time_ms),imuDataDownSampledNew.time_ms);
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
// 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 NavEKF3_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 NavEKF3_core::readBaroData()
{
// check to see if baro measurement has changed so we know if a new measurement has arrived
// limit update rate to avoid overflowing the FIFO buffer
if (frontend->_baro.get_last_update() - lastBaroReceived_ms > frontend->sensorIntervalMin_ms) {
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
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 NavEKF3_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 NavEKF3_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 -= MAX(gain * ekfOriginHgtVar , 0.0f);
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
}
}
/********************************************************
* Air Speed Measurements *
********************************************************/
// check for new airspeed data and update stored measurements if available
void NavEKF3_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) > frontend->sensorIntervalMin_ms) {
tasDataNew.tas = aspeed->get_raw_airspeed() * aspeed->get_EAS2TAS();
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
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 airspeed data and update stored measurements if available
void NavEKF3_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);
// Correct the range beacon earth frame origin for estimated offset relative to the EKF earth frame origin
if (rngBcnDataToFuse) {
rngBcnDataDelayed.beacon_posNED += bcnPosOffsetNED;
}
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
}
/*
update timing statistics structure
*/
void NavEKF3_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 NavEKF3_core::getTimingStatistics(struct ekf_timing &_timing)
{
_timing = timing;
memset(&timing, 0, sizeof(timing));
}
AP_NavEKF3: added EKF3 for EKF experimentation AP_NavEKF3: Implement same maths as PX4/ecl EKF Replace attitude vector states with quaternions Remove gyro scale factor states Add XY accel delta velocity bias estimation Initial tuning Add GPS body frame offset compensation AP_NavEKF3: Fix bugs and consolidate aiding switch logic Switching in and out of aiding modes was being performed in more than one place and was using two variables. The reversion out of GPS mode due to prolonged loss of GPS was not working. This consolidates the logic and ensures that PV_AidingMode is only changed by the setAidingMode function. AP_NavEKF3: prevent multiple fusion mode changes per filter update AP_NavEKF3: Update tuning defaults AP_NavEKF3: Fix bug causing switching in and out of aiding If the GPS receiver was disconnected and no data received, then then the gpsGoodToAlign check did not get a chance to run and becasue it was previously true the EKF would switch back into aiding. This prevents this by ensuring that gpsGoodToAlign defaults to false when the check is not being performed. An additional check has also been dded to ensure that there is GPS data to fuse before we declare ready to use GPS. AP_NavEKF3: Fix bug preventing planes recovering from bad magnetometers This bug created a race condition whereby if the EKF had to reset the yaw to the GPS ground course to recover from a bad magnetometer, the new heading could be over-written by the bad magnetic heading when the plane reached the height for the scheduled reset. AP_NavEKF3: Improve switch-over to backup magnetometer When switching over to a back up magnetometer, ensure that the earth field estimate are reset. Otherwise mag earth field estimates due to the previous failed mag could cause data from the new mag to be rejected. AP_NavEKF3: enable automatic use of range finder height AP_NavEKF3: Fix bug in handling of invalid range data AP_NavEKF3: Fix height drift on ground using range finder without GPSAP_NavEKF3: AP_NavEKF3: Handle yaw jumps due to core switches AP_NavEKF3: Enable simultaneous GPS and optical flow use AP_NavEKF3: fix console status reporting AP_NavEKF3: send messages to mavlink instead of console This allows the GCS to better handle the display of messages to the user. AP_NavEKF3: replace deprecated function call AP_NavEKF3: Compensate for sensor body frame offsets AP_NavEKF3: Fix bug in median filter code AP_NavEKF3: save some memory in the position offsets in EKF3 We don't need to copy that vector3f for every sample. A uint8_t does the job AP_NavEKF3: Add fusion of range beacon data AP_NavEKF3: Bring up to date with EKF2 AP_NavEKF3: Misc range beacon updates AP_NavEKF3: Add mising accessors AP_NavEKF3: remove duplicate include AP_NavEKF3: Prevent NaN's when accessing range beacon debug data AP_NavEKF3: Update range beacon naming AP_NavEKF3: updates AP_NavEKF3: miscellaneous changes AP_NavEKF3: misc updates AP_NavEKF3: misc range beacons updates AP_NavEKF3: add missing rover default param
2016-07-14 02:08:43 -03:00
#endif // HAL_CPU_CLASS