mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 06:28:27 -04:00
AP_NavEKF2: measurement buffer refactor
This commit is contained in:
Siddharth Bharat Purohit
Andrew Tridgell
parent
77a67d73e7
commit
b3c8dcee34
@ -490,7 +490,9 @@ bool NavEKF2::InitialiseFilter(void)
|
||||
num_cores = 0;
|
||||
for (uint8_t i=0; i<7; 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++;
|
||||
}
|
||||
}
|
||||
|
||||
@ -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
|
||||
// synthetic sidelip fusion only works for fixed wing aircraft and relies on the average sideslip being close to zero
|
||||
|
||||
@ -139,7 +139,7 @@ void NavEKF2_core::SelectMagFusion()
|
||||
}
|
||||
|
||||
// check for availability of magnetometer data to fuse
|
||||
magDataToFuse = RecallMag();
|
||||
magDataToFuse = storedMag.recall(magDataDelayed,imuDataDelayed.time_ms);
|
||||
|
||||
if (magDataToFuse) {
|
||||
// Control reset of yaw and magnetic field states
|
||||
|
||||
@ -71,14 +71,14 @@ void NavEKF2_core::readRangeFinder(void)
|
||||
rangeDataNew.rng = max(storedRngMeas[midIndex],rngOnGnd);
|
||||
rngValidMeaTime_ms = imuSampleTime_ms;
|
||||
// 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) {
|
||||
// before takeoff we assume on-ground range value if there is no data
|
||||
rangeDataNew.time_ms = imuSampleTime_ms;
|
||||
rangeDataNew.rng = rngOnGnd;
|
||||
rngValidMeaTime_ms = imuSampleTime_ms;
|
||||
// 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
|
||||
ofDataNew.time_ms = max(ofDataNew.time_ms,imuDataDelayed.time_ms);
|
||||
// Save data to buffer
|
||||
StoreOF();
|
||||
storedOF.push(ofDataNew, ofDataNew.time_ms);
|
||||
// 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 *
|
||||
@ -233,8 +192,8 @@ void NavEKF2_core::readMagData()
|
||||
// zero the learned magnetometer bias states
|
||||
stateStruct.body_magfield.zero();
|
||||
// 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();
|
||||
|
||||
// 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 *
|
||||
********************************************************/
|
||||
@ -380,7 +296,7 @@ void NavEKF2_core::readIMUData()
|
||||
// Time stamp the data
|
||||
imuDataDownSampledNew.time_ms = imuSampleTime_ms;
|
||||
// Write data to the FIFO IMU buffer
|
||||
StoreIMU();
|
||||
storedIMU.push(imuDataDownSampledNew, imuSampleTime_ms);
|
||||
// zero the accumulated IMU data and quaternion
|
||||
imuDataDownSampledNew.delAng.zero();
|
||||
imuDataDownSampledNew.delVel.zero();
|
||||
@ -398,50 +314,11 @@ void NavEKF2_core::readIMUData()
|
||||
}
|
||||
|
||||
// extract the oldest available data from the FIFO buffer
|
||||
imuDataDelayed = storedIMU[fifoIndexDelayed];
|
||||
|
||||
}
|
||||
|
||||
// store imu in the FIFO
|
||||
void NavEKF2_core::StoreIMU()
|
||||
{
|
||||
// increment the index and write new data
|
||||
fifoIndexNow = fifoIndexNow + 1;
|
||||
if (fifoIndexNow >= imu_buffer_length) {
|
||||
fifoIndexNow = 0;
|
||||
}
|
||||
storedIMU[fifoIndexNow] = 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
|
||||
imuDataDelayed = storedIMU.pop();
|
||||
float minDT = 0.1f*dtEkfAvg;
|
||||
imuDataDelayed.delAngDT = max(imuDataDelayed.delAngDT,minDT);
|
||||
imuDataDelayed.delVelDT = max(imuDataDelayed.delVelDT,minDT);
|
||||
|
||||
}
|
||||
|
||||
// read the delta velocity and corresponding time interval from the IMU
|
||||
@ -542,7 +419,7 @@ void NavEKF2_core::readGpsData()
|
||||
if (validOrigin) {
|
||||
gpsDataNew.pos = location_diff(EKF_origin, gpsloc);
|
||||
gpsDataNew.hgt = 0.01f * (gpsloc.alt - EKF_origin.alt);
|
||||
StoreGPS();
|
||||
storedGPS.push(gpsDataNew, gpsDataNew.time_ms);
|
||||
// declare GPS available for use
|
||||
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
|
||||
// return false if data is not available
|
||||
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);
|
||||
|
||||
// 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
|
||||
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 *
|
||||
********************************************************/
|
||||
@ -809,8 +564,8 @@ void NavEKF2_core::readAirSpdData()
|
||||
// Correct for the average intersampling delay due to the filter update rate
|
||||
tasDataNew.time_ms -= localFilterTimeStep_ms/2;
|
||||
newDataTas = true;
|
||||
StoreTAS();
|
||||
RecallTAS();
|
||||
storedTAS.push(tasDataNew, tasDataNew.time_ms);
|
||||
storedTAS.recall(tasDataDelayed,imuDataDelayed.time_ms);
|
||||
} else {
|
||||
newDataTas = false;
|
||||
}
|
||||
|
||||
@ -150,8 +150,7 @@ void NavEKF2_core::SelectVelPosFusion()
|
||||
|
||||
// read GPS data from the sensor and check for new data in the buffer
|
||||
readGpsData();
|
||||
gpsDataToFuse = RecallGPS();
|
||||
|
||||
gpsDataToFuse = storedGPS.recall(gpsDataDelayed,imuDataDelayed.time_ms);
|
||||
// Determine if we need to fuse position and velocity data on this time step
|
||||
if (gpsDataToFuse && PV_AidingMode == AID_ABSOLUTE) {
|
||||
// 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
|
||||
// This data is used by both height and optical flow fusion processing
|
||||
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
|
||||
readBaroData();
|
||||
baroDataToFuse = RecallBaro();
|
||||
baroDataToFuse = storedBaro.recall(baroDataDelayed, imuDataDelayed.time_ms);
|
||||
|
||||
// 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);
|
||||
|
||||
@ -55,7 +55,7 @@ NavEKF2_core::NavEKF2_core(void) :
|
||||
}
|
||||
|
||||
// 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;
|
||||
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;
|
||||
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];
|
||||
storedOutput = new output_elements[imu_buffer_length];
|
||||
return true;
|
||||
}
|
||||
|
||||
|
||||
@ -250,7 +273,8 @@ bool NavEKF2_core::InitialiseFilterBootstrap(void)
|
||||
dtIMUavg = 1.0f/_ahrs->get_ins().get_sample_rate();
|
||||
dtEkfAvg = min(0.01f,dtIMUavg);
|
||||
readIMUData();
|
||||
StoreIMU_reset();
|
||||
storedIMU.reset_history(imuDataNew, imuSampleTime_ms);
|
||||
imuDataDelayed = imuDataNew;
|
||||
|
||||
// acceleration vector in XYZ body axes measured by the IMU (m/s^2)
|
||||
Vector3f initAccVec;
|
||||
@ -556,11 +580,11 @@ void NavEKF2_core::calcOutputStatesFast() {
|
||||
|
||||
// store the output in the FIFO buffer if this is a filter update step
|
||||
if (runUpdates) {
|
||||
StoreOutput();
|
||||
storedOutput[storedIMU.get_head()] = outputDataNew;
|
||||
}
|
||||
|
||||
// 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
|
||||
|
||||
@ -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
|
||||
void NavEKF2_core::StoreOutputReset()
|
||||
{
|
||||
@ -1177,12 +1195,6 @@ void NavEKF2_core::StoreQuatRotate(Quaternion 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
|
||||
void NavEKF2_core::quat2Tbn(Matrix3f &Tbn, const Quaternion &quat) const
|
||||
{
|
||||
|
||||
@ -29,7 +29,7 @@
|
||||
|
||||
#include <AP_Math/AP_Math.h>
|
||||
#include "AP_NavEKF2.h"
|
||||
|
||||
#include <stdio.h>
|
||||
#include <AP_Math/vectorN.h>
|
||||
|
||||
// GPS pre-flight check bit locations
|
||||
@ -42,6 +42,119 @@
|
||||
#define MASK_GPS_VERT_SPD (1<<6)
|
||||
#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 NavEKF2_core
|
||||
@ -51,7 +164,7 @@ public:
|
||||
NavEKF2_core(void);
|
||||
|
||||
// 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)
|
||||
// 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
|
||||
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
|
||||
void StoreOutputReset(void);
|
||||
|
||||
@ -440,9 +541,6 @@ private:
|
||||
// Rotate the stored output quaternion history through a quaternion rotation
|
||||
void StoreQuatRotate(Quaternion deltaQuat);
|
||||
|
||||
// recall output data from the FIFO
|
||||
void RecallOutput();
|
||||
|
||||
// store altimeter data
|
||||
void StoreBaro();
|
||||
|
||||
@ -464,13 +562,6 @@ private:
|
||||
// return true if data found
|
||||
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
|
||||
void StoreTAS();
|
||||
|
||||
@ -659,13 +750,13 @@ private:
|
||||
Matrix24 KH; // intermediate result used for covariance updates
|
||||
Matrix24 KHP; // intermediate result used for covariance updates
|
||||
Matrix24 P; // covariance matrix
|
||||
imu_elements *storedIMU; // IMU data buffer [imu_buffer_length]
|
||||
gps_elements storedGPS[OBS_BUFFER_LENGTH]; // GPS data buffer
|
||||
mag_elements storedMag[OBS_BUFFER_LENGTH]; // Magnetometer data buffer
|
||||
baro_elements storedBaro[OBS_BUFFER_LENGTH]; // Baro data buffer
|
||||
range_elements storedRange[OBS_BUFFER_LENGTH]; // Rang finder data buffer
|
||||
tas_elements storedTAS[OBS_BUFFER_LENGTH]; // TAS data buffer
|
||||
output_elements *storedOutput; // output state buffer [imu_buffer_length]
|
||||
timed_ring_buffer_t<imu_elements> storedIMU; // IMU data buffer
|
||||
timed_ring_buffer_t<gps_elements> storedGPS; // GPS data buffer
|
||||
timed_ring_buffer_t<mag_elements> storedMag; // Magnetometer data buffer
|
||||
timed_ring_buffer_t<baro_elements> storedBaro; // Baro data buffer
|
||||
timed_ring_buffer_t<tas_elements> storedTAS; // TAS data buffer
|
||||
timed_ring_buffer_t<range_elements> storedRange;
|
||||
timed_ring_buffer_t<output_elements> storedOutput;// output state buffer
|
||||
Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad)
|
||||
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)
|
||||
@ -827,7 +918,7 @@ private:
|
||||
float lastInnovation;
|
||||
|
||||
// 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 ofDataDelayed; // OF data at the fusion time horizon
|
||||
uint8_t ofStoreIndex; // OF data storage index
|
||||
|
||||
Loading…
Reference in New Issue
Block a user