From b3c8dcee3482efc85963eef3d9ab75edd6c35d26 Mon Sep 17 00:00:00 2001 From: Siddharth Bharat Purohit Date: Fri, 13 Nov 2015 14:28:45 -0800 Subject: [PATCH] AP_NavEKF2: measurement buffer refactor --- libraries/AP_NavEKF2/AP_NavEKF2.cpp | 4 +- .../AP_NavEKF2/AP_NavEKF2_AirDataFusion.cpp | 37 --- libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp | 2 +- .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 273 +----------------- .../AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp | 7 +- libraries/AP_NavEKF2/AP_NavEKF2_core.cpp | 48 +-- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 155 ++++++++-- 7 files changed, 174 insertions(+), 352 deletions(-) diff --git a/libraries/AP_NavEKF2/AP_NavEKF2.cpp b/libraries/AP_NavEKF2/AP_NavEKF2.cpp index eb82f5fd38..dd3f1350c8 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2.cpp @@ -490,7 +490,9 @@ bool NavEKF2::InitialiseFilter(void) num_cores = 0; for (uint8_t i=0; i<7; i++) { if (_imuMask & (1U<= 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 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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp index 22ec887eb3..3313de4c81 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_MagFusion.cpp @@ -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 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index e9a94ba1e0..2c1c843971 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -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 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 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) { - 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 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 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 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; } diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp index e517882617..ea8b2035d4 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_PosVelFusion.cpp @@ -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); diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp index d417c83500..858c577406 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.cpp @@ -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 { diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index 7060a68341..c6ca79b92c 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -29,7 +29,7 @@ #include #include "AP_NavEKF2.h" - +#include #include // 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 +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 storedIMU; // IMU data buffer + timed_ring_buffer_t storedGPS; // GPS data buffer + timed_ring_buffer_t storedMag; // Magnetometer data buffer + timed_ring_buffer_t storedBaro; // Baro data buffer + timed_ring_buffer_t storedTAS; // TAS data buffer + timed_ring_buffer_t storedRange; + timed_ring_buffer_t 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 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