Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: measurement buffer refactor

Browse Source
This commit is contained in:
Siddharth Bharat Purohit 2015-11-13 14:28:45 -08:00 committed by Andrew Tridgell
parent 77a67d73e7
commit b3c8dcee34
7 changed files with 174 additions and 352 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
libraries/AP_NavEKF2/AP_NavEKF2.cpp
View File

@ -490,7 +490,9 @@ bool NavEKF2::InitialiseFilter(void)
num_cores = 0; num_cores = 0;
for (uint8_t i=0; i<7; i++) { for (uint8_t i=0; i<7; i++) {
if (_imuMask & (1U<<i)) { if (_imuMask & (1U<<i)) {
core[num_cores].setup_core(this, i, num_cores); if(!core[num_cores].setup_core(this, i, num_cores)) {
return false;
}
num_cores++; num_cores++;
} }
} }

37
libraries/AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp
View File

@ -222,43 +222,6 @@ void NavEKF2_core::SelectTasFusion()
} }
} }
// store TAS in a history array
void NavEKF2_core::StoreTAS()
{
if (tasStoreIndex >= OBS_BUFFER_LENGTH) {
tasStoreIndex = 0;
}
storedTAS[tasStoreIndex] = tasDataNew;
tasStoreIndex += 1;
}
// return newest un-used true airspeed data that has fallen behind the fusion time horizon
// if no un-used data is available behind the fusion horizon, return false
bool NavEKF2_core::RecallTAS()
{
tas_elements dataTemp;
tas_elements dataTempZero;
dataTempZero.time_ms = 0;
uint32_t temp_ms = 0;
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
dataTemp = storedTAS[i];
// find a measurement older than the fusion time horizon that we haven't checked before
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
// zero the time stamp so we won't use it again
storedTAS[i]=dataTempZero;
// Find the most recent non-stale measurement that meets the time horizon criteria
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
tasDataDelayed = dataTemp;
temp_ms = dataTemp.time_ms;
}
}
}
if (temp_ms != 0) {
return true;
} else {
return false;
}
}
// select fusion of synthetic sideslip measurements // select fusion of synthetic sideslip measurements
// synthetic sidelip fusion only works for fixed wing aircraft and relies on the average sideslip being close to zero // synthetic sidelip fusion only works for fixed wing aircraft and relies on the average sideslip being close to zero

2
libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp
View File

@ -139,7 +139,7 @@ void NavEKF2_core::SelectMagFusion()
} }
// check for availability of magnetometer data to fuse // check for availability of magnetometer data to fuse
magDataToFuse = RecallMag(); magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms);
if (magDataToFuse) { if (magDataToFuse) {
// Control reset of yaw and magnetic field states // Control reset of yaw and magnetic field states

273
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -71,14 +71,14 @@ void NavEKF2_core::readRangeFinder(void)
rangeDataNew.rng = max(storedRngMeas[midIndex],rngOnGnd); rangeDataNew.rng = max(storedRngMeas[midIndex],rngOnGnd);
rngValidMeaTime_ms = imuSampleTime_ms; rngValidMeaTime_ms = imuSampleTime_ms;
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
StoreRange(); storedRange.push(rangeDataNew,imuSampleTime_ms);
} else if (!takeOffDetected) { } else if (!takeOffDetected) {
// before takeoff we assume on-ground range value if there is no data // before takeoff we assume on-ground range value if there is no data
rangeDataNew.time_ms = imuSampleTime_ms; rangeDataNew.time_ms = imuSampleTime_ms;
rangeDataNew.rng = rngOnGnd; rangeDataNew.rng = rngOnGnd;
rngValidMeaTime_ms = imuSampleTime_ms; rngValidMeaTime_ms = imuSampleTime_ms;
// write data to buffer with time stamp to be fused when the fusion time horizon catches up with it // write data to buffer with time stamp to be fused when the fusion time horizon catches up with it
StoreRange(); storedRange.push(rangeDataNew,imuSampleTime_ms);
} }
} }
} }
@ -131,53 +131,12 @@ void NavEKF2_core::writeOptFlowMeas(uint8_t &rawFlowQuality, Vector2f &rawFlowRa
// Prevent time delay exceeding age of oldest IMU data in the buffer // Prevent time delay exceeding age of oldest IMU data in the buffer
ofDataNew.time_ms = max(ofDataNew.time_ms,imuDataDelayed.time_ms); ofDataNew.time_ms = max(ofDataNew.time_ms,imuDataDelayed.time_ms);
// Save data to buffer // Save data to buffer
StoreOF(); storedOF.push(ofDataNew, ofDataNew.time_ms);
// Check for data at the fusion time horizon // Check for data at the fusion time horizon
flowDataToFuse = RecallOF(); flowDataToFuse = storedOF.recall(ofDataDelayed, imuDataDelayed.time_ms);
} }
} }
// store OF data in a history array
void NavEKF2_core::StoreOF()
{
if (ofStoreIndex >= OBS_BUFFER_LENGTH) {
ofStoreIndex = 0;
}
storedOF[ofStoreIndex] = ofDataNew;
ofStoreIndex += 1;
}
// return newest un-used optical flow data that has fallen behind the fusion time horizon
// if no un-used data is available behind the fusion horizon, return false
bool NavEKF2_core::RecallOF()
{
of_elements dataTemp;
of_elements dataTempZero;
dataTempZero.time_ms = 0;
uint32_t temp_ms = 0;
uint8_t bestIndex = 0;
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
dataTemp = storedOF[i];
// find a measurement older than the fusion time horizon that we haven't checked before
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
// Find the most recent non-stale measurement that meets the time horizon criteria
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
ofDataDelayed = dataTemp;
temp_ms = dataTemp.time_ms;
bestIndex = i;
}
}
}
if (temp_ms != 0) {
// zero the time stamp for that piece of data so we won't use it again
storedOF[bestIndex]=dataTempZero;
return true;
} else {
return false;
}
}
/******************************************************** /********************************************************
* MAGNETOMETER * * MAGNETOMETER *
@ -233,8 +192,8 @@ void NavEKF2_core::readMagData()
// zero the learned magnetometer bias states // zero the learned magnetometer bias states
stateStruct.body_magfield.zero(); stateStruct.body_magfield.zero();
// clear the measurement buffer // clear the measurement buffer
memset(&storedMag[0], 0, sizeof(storedMag)); storedMag.reset();
} }
} }
} }
@ -254,53 +213,10 @@ void NavEKF2_core::readMagData()
consistentMagData = _ahrs->get_compass()->consistent(); consistentMagData = _ahrs->get_compass()->consistent();
// save magnetometer measurement to buffer to be fused later // save magnetometer measurement to buffer to be fused later
StoreMag(); storedMag.push(magDataNew, magDataNew.time_ms);
} }
} }
// store magnetometer data in a history array
void NavEKF2_core::StoreMag()
{
if (magStoreIndex >= OBS_BUFFER_LENGTH) {
magStoreIndex = 0;
}
storedMag[magStoreIndex] = magDataNew;
magStoreIndex += 1;
}
// return newest un-used magnetometer data that has fallen behind the fusion time horizon
// if no un-used data is available behind the fusion horizon, return false
bool NavEKF2_core::RecallMag()
{
mag_elements dataTemp;
mag_elements dataTempZero;
dataTempZero.time_ms = 0;
uint32_t temp_ms = 0;
uint8_t bestIndex = 0;
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
dataTemp = storedMag[i];
// find a measurement older than the fusion time horizon that we haven't checked before
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
// Find the most recent non-stale measurement that meets the time horizon criteria
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
magDataDelayed = dataTemp;
temp_ms = dataTemp.time_ms;
bestIndex = i;
}
}
}
if (temp_ms != 0) {
// zero the time stamp for that piece of data so we won't use it again
storedMag[bestIndex]=dataTempZero;
return true;
} else {
return false;
}
}
/******************************************************** /********************************************************
* Inertial Measurements * * Inertial Measurements *
********************************************************/ ********************************************************/
@ -380,7 +296,7 @@ void NavEKF2_core::readIMUData()
// Time stamp the data // Time stamp the data
imuDataDownSampledNew.time_ms = imuSampleTime_ms; imuDataDownSampledNew.time_ms = imuSampleTime_ms;
// Write data to the FIFO IMU buffer // Write data to the FIFO IMU buffer
StoreIMU(); storedIMU.push(imuDataDownSampledNew, imuSampleTime_ms);
// zero the accumulated IMU data and quaternion // zero the accumulated IMU data and quaternion
imuDataDownSampledNew.delAng.zero(); imuDataDownSampledNew.delAng.zero();
imuDataDownSampledNew.delVel.zero(); imuDataDownSampledNew.delVel.zero();
@ -398,50 +314,11 @@ void NavEKF2_core::readIMUData()
} }
// extract the oldest available data from the FIFO buffer // extract the oldest available data from the FIFO buffer
imuDataDelayed = storedIMU[fifoIndexDelayed]; imuDataDelayed = storedIMU.pop();
}
// store imu in the FIFO
void NavEKF2_core::StoreIMU()
{
// increment the index and write new data
fifoIndexNow = fifoIndexNow + 1;
if (fifoIndexNow >= imu_buffer_length) {
fifoIndexNow = 0;
}
storedIMU[fifoIndexNow] = imuDataDownSampledNew;
// set the index required to access the oldest data, applying an offset to the fusion time horizon that is used to
// prevent the same fusion operation being performed on the same frame across multiple EKF's
fifoIndexDelayed = fifoIndexNow + 1;
if (fifoIndexDelayed >= imu_buffer_length) {
fifoIndexDelayed = 0;
}
}
// reset the stored imu history and store the current value
void NavEKF2_core::StoreIMU_reset()
{
// write current measurement to entire table
for (uint8_t i=0; i<imu_buffer_length; i++) {
storedIMU[i] = imuDataNew;
}
imuDataDelayed = imuDataNew;
fifoIndexDelayed = fifoIndexNow+1;
if (fifoIndexDelayed >= imu_buffer_length) {
fifoIndexDelayed = 0;
}
}
// recall IMU data from the FIFO
void NavEKF2_core::RecallIMU()
{
imuDataDelayed = storedIMU[fifoIndexDelayed];
// make sure that the delta time used for the delta angles and velocities are is no less than 10% of dtIMUavg to prevent
// divide by zero problems when converting to rates or acceleration
float minDT = 0.1f*dtEkfAvg; float minDT = 0.1f*dtEkfAvg;
imuDataDelayed.delAngDT = max(imuDataDelayed.delAngDT,minDT); imuDataDelayed.delAngDT = max(imuDataDelayed.delAngDT,minDT);
imuDataDelayed.delVelDT = max(imuDataDelayed.delVelDT,minDT); imuDataDelayed.delVelDT = max(imuDataDelayed.delVelDT,minDT);
} }
// read the delta velocity and corresponding time interval from the IMU // read the delta velocity and corresponding time interval from the IMU
@ -542,7 +419,7 @@ void NavEKF2_core::readGpsData()
if (validOrigin) { if (validOrigin) {
gpsDataNew.pos = location_diff(EKF_origin, gpsloc); gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt); gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt);
StoreGPS(); storedGPS.push(gpsDataNew, gpsDataNew.time_ms);
// declare GPS available for use // declare GPS available for use
gpsNotAvailable = false; gpsNotAvailable = false;
} }
@ -610,47 +487,6 @@ void NavEKF2_core::readGpsData()
} }
} }
// store GPS data in a history array
void NavEKF2_core::StoreGPS()
{
if (gpsStoreIndex >= OBS_BUFFER_LENGTH) {
gpsStoreIndex = 0;
}
storedGPS[gpsStoreIndex] = gpsDataNew;
gpsStoreIndex += 1;
}
// return newest un-used GPS data that has fallen behind the fusion time horizon
// if no un-used data is available behind the fusion horizon, return false
bool NavEKF2_core::RecallGPS()
{
gps_elements dataTemp;
gps_elements dataTempZero;
dataTempZero.time_ms = 0;
uint32_t temp_ms = 0;
uint8_t bestIndex;
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
dataTemp = storedGPS[i];
// find a measurement older than the fusion time horizon that we haven't checked before
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
// Find the most recent non-stale measurement that meets the time horizon criteria
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
gpsDataDelayed = dataTemp;
temp_ms = dataTemp.time_ms;
bestIndex = i;
}
}
}
if (temp_ms != 0) {
// zero the time stamp for that piece of data so we won't use it again
storedGPS[bestIndex]=dataTempZero;
return true;
} else {
return false;
}
}
// read the delta angle and corresponding time interval from the IMU // read the delta angle and corresponding time interval from the IMU
// return false if data is not available // return false if data is not available
bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) { bool NavEKF2_core::readDeltaAngle(uint8_t ins_index, Vector3f &dAng) {
@ -696,7 +532,7 @@ void NavEKF2_core::readBaroData()
baroDataNew.time_ms = max(baroDataNew.time_ms,imuDataDelayed.time_ms); baroDataNew.time_ms = max(baroDataNew.time_ms,imuDataDelayed.time_ms);
// save baro measurement to buffer to be fused later // save baro measurement to buffer to be fused later
StoreBaro(); storedBaro.push(baroDataNew,baroDataNew.time_ms);
} }
} }
@ -708,87 +544,6 @@ void NavEKF2_core::calcFiltBaroOffset()
// Apply a first order LPF with spike protection // Apply a first order LPF with spike protection
baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f); baroHgtOffset += 0.1f * constrain_float(baroDataDelayed.hgt + stateStruct.position.z - baroHgtOffset, -5.0f, 5.0f);
} }
// store baro in a history array
void NavEKF2_core::StoreBaro()
{
if (baroStoreIndex >= OBS_BUFFER_LENGTH) {
baroStoreIndex = 0;
}
storedBaro[baroStoreIndex] = baroDataNew;
baroStoreIndex += 1;
}
// return newest un-used baro data that has fallen behind the fusion time horizon
// if no un-used data is available behind the fusion horizon, return false
bool NavEKF2_core::RecallBaro()
{
baro_elements dataTemp;
baro_elements dataTempZero;
dataTempZero.time_ms = 0;
uint32_t temp_ms = 0;
uint8_t bestIndex = 0;
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
dataTemp = storedBaro[i];
// find a measurement older than the fusion time horizon that we haven't checked before
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
// Find the most recent non-stale measurement that meets the time horizon criteria
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
baroDataDelayed = dataTemp;
temp_ms = dataTemp.time_ms;
bestIndex = i;
}
}
}
if (temp_ms != 0) {
// zero the time stamp for that piece of data so we won't use it again
storedBaro[bestIndex]=dataTempZero;
return true;
} else {
return false;
}
}
// store baro in a history array
void NavEKF2_core::StoreRange()
{
if (rangeStoreIndex >= OBS_BUFFER_LENGTH) {
rangeStoreIndex = 0;
}
storedRange[rangeStoreIndex] = rangeDataNew;
rangeStoreIndex += 1;
}
// return newest un-used range finder data that has fallen behind the fusion time horizon
// if no un-used data is available behind the fusion horizon, return false
bool NavEKF2_core::RecallRange()
{
range_elements dataTemp;
range_elements dataTempZero;
dataTempZero.time_ms = 0;
uint32_t temp_ms = 0;
uint8_t bestIndex = 0;
for (uint8_t i=0; i<OBS_BUFFER_LENGTH; i++) {
dataTemp = storedRange[i];
// find a measurement older than the fusion time horizon that we haven't checked before
if (dataTemp.time_ms != 0 && dataTemp.time_ms <= imuDataDelayed.time_ms) {
// Find the most recent non-stale measurement that meets the time horizon criteria
if (((imuDataDelayed.time_ms - dataTemp.time_ms) < 500) && dataTemp.time_ms > temp_ms) {
rangeDataDelayed = dataTemp;
temp_ms = dataTemp.time_ms;
bestIndex = i;
}
}
}
if (temp_ms != 0) {
// zero the time stamp for that piece of data so we won't use it again
storedRange[bestIndex]=dataTempZero;
return true;
} else {
return false;
}
}
/******************************************************** /********************************************************
* Air Speed Measurements * * Air Speed Measurements *
********************************************************/ ********************************************************/
@ -809,8 +564,8 @@ void NavEKF2_core::readAirSpdData()
// Correct for the average intersampling delay due to the filter update rate // Correct for the average intersampling delay due to the filter update rate
tasDataNew.time_ms -= localFilterTimeStep_ms/2; tasDataNew.time_ms -= localFilterTimeStep_ms/2;
newDataTas = true; newDataTas = true;
StoreTAS(); storedTAS.push(tasDataNew, tasDataNew.time_ms);
RecallTAS(); storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
} else { } else {
newDataTas = false; newDataTas = false;
} }

7
libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp
View File

@ -150,8 +150,7 @@ void NavEKF2_core::SelectVelPosFusion()
// read GPS data from the sensor and check for new data in the buffer // read GPS data from the sensor and check for new data in the buffer
readGpsData(); readGpsData();
gpsDataToFuse = RecallGPS(); gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
// Determine if we need to fuse position and velocity data on this time step // Determine if we need to fuse position and velocity data on this time step
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) { if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
// Don't fuse velocity data if GPS doesn't support it // Don't fuse velocity data if GPS doesn't support it
@ -550,11 +549,11 @@ void NavEKF2_core::selectHeightForFusion()
// Read range finder data and check for new data in the buffer // Read range finder data and check for new data in the buffer
// This data is used by both height and optical flow fusion processing // This data is used by both height and optical flow fusion processing
readRangeFinder(); readRangeFinder();
rangeDataToFuse = RecallRange(); rangeDataToFuse = storedRange.recall(rangeDataDelayed,imuDataDelayed.time_ms);
// read baro height data from the sensor and check for new data in the buffer // read baro height data from the sensor and check for new data in the buffer
readBaroData(); readBaroData();
baroDataToFuse = RecallBaro(); baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
// determine if we should be using a height source other than baro // determine if we should be using a height source other than baro
bool usingRangeForHgt = (frontend->_altSource == 1 && imuSampleTime_ms - rngValidMeaTime_ms < 500 && frontend->_fusionModeGPS == 3); bool usingRangeForHgt = (frontend->_altSource == 1 && imuSampleTime_ms - rngValidMeaTime_ms < 500 && frontend->_fusionModeGPS == 3);

48
libraries/AP_NavEKF2/AP_NavEKF2_core.cpp
View File

@ -55,7 +55,7 @@ NavEKF2_core::NavEKF2_core(void) :
} }
// setup this core backend // setup this core backend
void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index) bool NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index)
{ {
frontend = _frontend; frontend = _frontend;
imu_index = _imu_index; imu_index = _imu_index;
@ -79,9 +79,32 @@ void NavEKF2_core::setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _c
imu_buffer_length = 26; imu_buffer_length = 26;
break; break;
} }
if(!storedGPS.init(OBS_BUFFER_LENGTH)) {
return false;
}
if(!storedMag.init(OBS_BUFFER_LENGTH)) {
return false;
}
if(!storedBaro.init(OBS_BUFFER_LENGTH)) {
return false;
}
if(!storedTAS.init(OBS_BUFFER_LENGTH)) {
return false;
}
if(!storedOF.init(OBS_BUFFER_LENGTH)) {
return false;
}
if(!storedRange.init(OBS_BUFFER_LENGTH)) {
return false;
}
if(!storedIMU.init(imu_buffer_length)) {
return false;
}
if(!storedOutput.init(imu_buffer_length)) {
return false;
}
storedIMU = new imu_elements[imu_buffer_length]; return true;
storedOutput = new output_elements[imu_buffer_length];
} }
@ -250,7 +273,8 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
dtIMUavg = 1.0f/_ahrs->get_ins().get_sample_rate(); dtIMUavg = 1.0f/_ahrs->get_ins().get_sample_rate();
dtEkfAvg = min(0.01f,dtIMUavg); dtEkfAvg = min(0.01f,dtIMUavg);
readIMUData(); readIMUData();
StoreIMU_reset(); storedIMU.reset_history(imuDataNew, imuSampleTime_ms);
imuDataDelayed = imuDataNew;
// acceleration vector in XYZ body axes measured by the IMU (m/s^2) // acceleration vector in XYZ body axes measured by the IMU (m/s^2)
Vector3f initAccVec; Vector3f initAccVec;
@ -556,11 +580,11 @@ void NavEKF2_core::calcOutputStatesFast() {
// store the output in the FIFO buffer if this is a filter update step // store the output in the FIFO buffer if this is a filter update step
if (runUpdates) { if (runUpdates) {
StoreOutput(); storedOutput[storedIMU.get_head()] = outputDataNew;
} }
// extract data at the fusion time horizon from the FIFO buffer // extract data at the fusion time horizon from the FIFO buffer
RecallOutput(); outputDataDelayed = storedOutput[storedIMU.get_tail()];
// compare quaternion data with EKF quaternion at the fusion time horizon and calculate correction // compare quaternion data with EKF quaternion at the fusion time horizon and calculate correction
@ -1133,12 +1157,6 @@ void NavEKF2_core::zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last)
} }
} }
// store output data in the FIFO
void NavEKF2_core::StoreOutput()
{
storedOutput[fifoIndexNow] = outputDataNew;
}
// reset the output data to the current EKF state // reset the output data to the current EKF state
void NavEKF2_core::StoreOutputReset() void NavEKF2_core::StoreOutputReset()
{ {
@ -1177,12 +1195,6 @@ void NavEKF2_core::StoreQuatRotate(Quaternion deltaQuat)
outputDataDelayed.quat = outputDataDelayed.quat*deltaQuat; outputDataDelayed.quat = outputDataDelayed.quat*deltaQuat;
} }
// recall output data from the FIFO
void NavEKF2_core::RecallOutput()
{
outputDataDelayed = storedOutput[fifoIndexDelayed];
}
// calculate nav to body quaternions from body to nav rotation matrix // calculate nav to body quaternions from body to nav rotation matrix
void NavEKF2_core::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const void NavEKF2_core::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const
{ {

155
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -29,7 +29,7 @@
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#include "AP_NavEKF2.h" #include "AP_NavEKF2.h"
#include <stdio.h>
#include <AP_Math/vectorN.h> #include <AP_Math/vectorN.h>
// GPS pre-flight check bit locations // GPS pre-flight check bit locations
@ -42,6 +42,119 @@
#define MASK_GPS_VERT_SPD (1<<6) #define MASK_GPS_VERT_SPD (1<<6)
#define MASK_GPS_HORIZ_SPD (1<<7) #define MASK_GPS_HORIZ_SPD (1<<7)
template <typename element_type>
class timed_ring_buffer_t
{
public:
struct element_t{
element_type element;
uint32_t sample_time;
} *buffer;
bool init(uint32_t size)
{
buffer = new element_t[size];
memset(buffer,0,_size*sizeof(element_t));
if(buffer == NULL)
{
return false;
}
_size = size;
_head = 0;
_tail = 0;
return true;
}
void sorted_store(element_type element, uint32_t sample_time)
{
uint8_t head = _head;
//will drop the element if older than tail i.e. recently fetched data
while(head != _tail) {
if(buffer[(head - 1)%_size].sample_time < sample_time) {
buffer[head].element = element;
} else {
buffer[head] = buffer[(head - 1)%_size];
head = (head-1)%_size;
}
}
_head = (_head+1)%_size;
return;
}
bool recall(element_type &element,uint32_t sample_time)
{
bool success = false;
uint8_t tail = _tail, bestIndex;
while (_head != tail) {
// find a measurement older than the fusion time horizon that we haven't checked before
if (buffer[tail].sample_time != 0 && buffer[tail].sample_time <= sample_time) {
// Find the most recent non-stale measurement that meets the time horizon criteria
if (((sample_time - buffer[tail].sample_time) < 500)) {
bestIndex = tail;
success = true;
}
} else if(buffer[tail].sample_time > sample_time){
break;
}
tail = (tail+1)%_size;
}
if (success) {
// zero the time stamp for that piece of data so we won't use it again
element = buffer[bestIndex].element;
element.time_ms = buffer[bestIndex].sample_time;
_tail=(bestIndex+1)%_size;
return true;
} else {
return false;
}
}
inline void push(element_type element, uint32_t sample_time)
{
buffer[_head].element = element;
buffer[_head].sample_time = sample_time;
_head = (_head+1)%_size;
}
inline void push(element_type element)
{
buffer[_head].element = element;
_head = (_head+1)%_size;
}
inline element_type pop() {
element_type ret = buffer[_tail].element;
if(_head != _tail) {
_tail = (_tail+1)%_size;
}
return ret;
}
inline void reset_history(element_type element, uint32_t sample_time) {
_head = (_tail+1)%_size;
buffer[_tail].sample_time = sample_time;
buffer[_tail].element = element;
}
inline void reset() {
_head=_tail=0;
memset(buffer,0,_size*sizeof(element_t));
}
inline element_type& operator[](uint32_t index) {
return buffer[index].element;
}
inline uint8_t get_tail(){
return _tail;
}
inline uint8_t get_head(){
return _head;
}
private:
uint8_t _size,_head,_tail;
};
class AP_AHRS; class AP_AHRS;
class NavEKF2_core class NavEKF2_core
@ -51,7 +164,7 @@ public:
NavEKF2_core(void); NavEKF2_core(void);
// setup this core backend // setup this core backend
void setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index); bool setup_core(NavEKF2 *_frontend, uint8_t _imu_index, uint8_t _core_index);
// Initialise the states from accelerometer and magnetometer data (if present) // Initialise the states from accelerometer and magnetometer data (if present)
// This method can only be used when the vehicle is static // This method can only be used when the vehicle is static
@ -419,18 +532,6 @@ private:
// zero specified range of columns in the state covariance matrix // zero specified range of columns in the state covariance matrix
void zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last); void zeroCols(Matrix24 &covMat, uint8_t first, uint8_t last);
// store imu data in the FIFO
void StoreIMU(void);
// Reset the stored IMU history to current data
void StoreIMU_reset(void);
// recall IMU data from the FIFO
void RecallIMU();
// store output data in the FIFO
void StoreOutput(void);
// Reset the stored output history to current data // Reset the stored output history to current data
void StoreOutputReset(void); void StoreOutputReset(void);
@ -440,9 +541,6 @@ private:
// Rotate the stored output quaternion history through a quaternion rotation // Rotate the stored output quaternion history through a quaternion rotation
void StoreQuatRotate(Quaternion deltaQuat); void StoreQuatRotate(Quaternion deltaQuat);
// recall output data from the FIFO
void RecallOutput();
// store altimeter data // store altimeter data
void StoreBaro(); void StoreBaro();
@ -464,13 +562,6 @@ private:
// return true if data found // return true if data found
bool RecallMag(); bool RecallMag();
// store GPS data
void StoreGPS();
// recall GPS data at the fusion time horizon
// return true if data found
bool RecallGPS();
// store true airspeed data // store true airspeed data
void StoreTAS(); void StoreTAS();
@ -659,13 +750,13 @@ private:
Matrix24 KH; // intermediate result used for covariance updates Matrix24 KH; // intermediate result used for covariance updates
Matrix24 KHP; // intermediate result used for covariance updates Matrix24 KHP; // intermediate result used for covariance updates
Matrix24 P; // covariance matrix Matrix24 P; // covariance matrix
imu_elements *storedIMU; // IMU data buffer [imu_buffer_length] timed_ring_buffer_t<imu_elements> storedIMU; // IMU data buffer
gps_elements storedGPS[OBS_BUFFER_LENGTH]; // GPS data buffer timed_ring_buffer_t<gps_elements> storedGPS; // GPS data buffer
mag_elements storedMag[OBS_BUFFER_LENGTH]; // Magnetometer data buffer timed_ring_buffer_t<mag_elements> storedMag; // Magnetometer data buffer
baro_elements storedBaro[OBS_BUFFER_LENGTH]; // Baro data buffer timed_ring_buffer_t<baro_elements> storedBaro; // Baro data buffer
range_elements storedRange[OBS_BUFFER_LENGTH]; // Rang finder data buffer timed_ring_buffer_t<tas_elements> storedTAS; // TAS data buffer
tas_elements storedTAS[OBS_BUFFER_LENGTH]; // TAS data buffer timed_ring_buffer_t<range_elements> storedRange;
output_elements *storedOutput; // output state buffer [imu_buffer_length] timed_ring_buffer_t<output_elements> storedOutput;// output state buffer
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
Quaternion correctedDelAngQuat; // quaternion representation of correctedDelAng Quaternion correctedDelAngQuat; // quaternion representation of correctedDelAng
Vector3f correctedDelVel; // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s) Vector3f correctedDelVel; // delta velocities along the XYZ body axes for weighted average of IMU1 and IMU2 corrected for errors (m/s)
@ -827,7 +918,7 @@ private:
float lastInnovation; float lastInnovation;
// variables added for optical flow fusion // variables added for optical flow fusion
of_elements storedOF[OBS_BUFFER_LENGTH]; // OF data buffer timed_ring_buffer_t<of_elements> storedOF; // OF data buffer
of_elements ofDataNew; // OF data at the current time horizon of_elements ofDataNew; // OF data at the current time horizon
of_elements ofDataDelayed; // OF data at the fusion time horizon of_elements ofDataDelayed; // OF data at the fusion time horizon
uint8_t ofStoreIndex; // OF data storage index uint8_t ofStoreIndex; // OF data storage index