Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF3: convert to using common buffer classes 

this saves a considerable amount of flash
This commit is contained in:
Andrew Tridgell 2020-11-14 16:11:21 +11:00
parent 39ba8a8c83
commit 1e4b1d7563
3 changed files with 35 additions and 246 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

200
libraries/AP_NavEKF3/AP_NavEKF3_Buffer.h
View File

@ -1,200 +0,0 @@
// EKF Buffer models
// this buffer model is to be used for observation buffers,
// the data is pushed into buffer like any standard ring buffer
// return is based on the sample time provided
template <typename element_type>
class obs_ring_buffer_t
{
public:
struct element_t{
element_type element;
} *buffer;
// initialise buffer, returns false when allocation has failed
bool init(uint32_t size)
{
buffer = new element_t[size];
if(buffer == nullptr)
{
return false;
}
memset((void *)buffer,0,size*sizeof(element_t));
_size = size;
_head = 0;
_tail = 0;
_new_data = false;
return true;
}
/*
* Searches through a ring buffer and return the newest data that is older than the
* time specified by sample_time_ms
* Zeros old data so it cannot not be used again
* Returns false if no data can be found that is less than 100msec old
*/
bool recall(element_type &element,uint32_t sample_time)
{
if(!_new_data) {
return false;
}
bool success = false;
uint8_t tail = _tail, bestIndex;
if(_head == tail) {
if (buffer[tail].element.time_ms != 0 && buffer[tail].element.time_ms <= sample_time) {
// if head is equal to tail just check if the data is unused and within time horizon window
if (((sample_time - buffer[tail].element.time_ms) < 100)) {
bestIndex = tail;
success = true;
_new_data = false;
}
}
} else {
while(_head != tail) {
// find a measurement older than the fusion time horizon that we haven't checked before
if (buffer[tail].element.time_ms != 0 && buffer[tail].element.time_ms <= sample_time) {
// Find the most recent non-stale measurement that meets the time horizon criteria
if (((sample_time - buffer[tail].element.time_ms) < 100)) {
bestIndex = tail;
success = true;
}
} else if(buffer[tail].element.time_ms > sample_time){
break;
}
tail = (tail+1)%_size;
}
}
if (success) {
element = buffer[bestIndex].element;
_tail = (bestIndex+1)%_size;
//make time zero to stop using it again,
//resolves corner case of reusing the element when head == tail
buffer[bestIndex].element.time_ms = 0;
return true;
} else {
return false;
}
}
/*
* Writes data and timestamp to a Ring buffer and advances indices that
* define the location of the newest and oldest data
*/
inline void push(element_type element)
{
if (buffer == nullptr) {
return;
}
// Advance head to next available index
_head = (_head+1)%_size;
// New data is written at the head
buffer[_head].element = element;
_new_data = true;
}
// writes the same data to all elements in the ring buffer
inline void reset_history(element_type element, uint32_t sample_time) {
for (uint8_t index=0; index<_size; index++) {
buffer[index].element = element;
}
}
// zeroes all data in the ring buffer
inline void reset() {
_head = 0;
_tail = 0;
_new_data = false;
memset((void *)buffer,0,_size*sizeof(element_t));
}
private:
uint8_t _size,_head,_tail,_new_data;
};
// Following buffer model is for IMU data,
// it achieves a distance of sample size
// between youngest and oldest
template <typename element_type>
class imu_ring_buffer_t
{
public:
struct element_t{
element_type element;
} *buffer;
// initialise buffer, returns false when allocation has failed
bool init(uint32_t size)
{
buffer = new element_t[size];
if(buffer == nullptr)
{
return false;
}
memset((void *)buffer,0,size*sizeof(element_t));
_size = size;
_youngest = 0;
_oldest = 0;
return true;
}
/*
* Writes data to a Ring buffer and advances indices that
* define the location of the newest and oldest data
*/
inline void push_youngest_element(element_type element)
{
// push youngest to the buffer
_youngest = (_youngest+1)%_size;
buffer[_youngest].element = element;
// set oldest data index
_oldest = (_youngest+1)%_size;
if (_oldest == 0) {
_filled = true;
}
}
// return true if the buffer has been filled at least once
inline bool is_filled(void) const {
return _filled;
}
// retrieve the oldest data from the ring buffer tail
inline element_type pop_oldest_element() {
element_type ret = buffer[_oldest].element;
return ret;
}
// writes the same data to all elements in the ring buffer
inline void reset_history(element_type element) {
for (uint8_t index=0; index<_size; index++) {
buffer[index].element = element;
}
}
// zeroes all data in the ring buffer
inline void reset() {
_youngest = 0;
_oldest = 0;
memset((void *)buffer,0,_size*sizeof(element_t));
}
// retrieves data from the ring buffer at a specified index
inline element_type& operator[](uint32_t index) {
return buffer[index].element;
}
// returns the index for the ring buffer oldest data
inline uint8_t get_oldest_index(){
return _oldest;
}
// returns the index for the ring buffer youngest data
inline uint8_t get_youngest_index(){
return _youngest;
}
private:
uint8_t _size,_oldest,_youngest;
bool _filled;
};

15
libraries/AP_NavEKF3/AP_NavEKF3_Measurements.cpp
View File

@ -497,7 +497,7 @@ void NavEKF3_core::readIMUData()
runUpdates = true; runUpdates = true;
// extract the oldest available data from the FIFO buffer // extract the oldest available data from the FIFO buffer
imuDataDelayed = storedIMU.pop_oldest_element(); imuDataDelayed = storedIMU.get_oldest_element();
// protect against delta time going to zero // protect against delta time going to zero
float minDT = 0.1f * dtEkfAvg; float minDT = 0.1f * dtEkfAvg;
@ -1040,12 +1040,13 @@ void NavEKF3_core::writeExtNavVelData(const Vector3f &vel, float err, uint32_t t
timeStamp_ms -= localFilterTimeStep_ms/2; timeStamp_ms -= localFilterTimeStep_ms/2;
// Prevent time delay exceeding age of oldest IMU data in the buffer // Prevent time delay exceeding age of oldest IMU data in the buffer
timeStamp_ms = MAX(timeStamp_ms,imuDataDelayed.time_ms); timeStamp_ms = MAX(timeStamp_ms,imuDataDelayed.time_ms);
const ext_nav_vel_elements extNavVelNew {
vel, ext_nav_vel_elements extNavVelNew;
err, extNavVelNew.time_ms = timeStamp_ms;
timeStamp_ms, extNavVelNew.vel = vel;
false extNavVelNew.err = err;
}; extNavVelNew.corrected = false;
storedExtNavVel.push(extNavVelNew); storedExtNavVel.push(extNavVelNew);
} }

66
libraries/AP_NavEKF3/AP_NavEKF3_core.h
View File

@ -29,7 +29,7 @@
#include <AP_Math/vectorN.h> #include <AP_Math/vectorN.h>
#include <AP_NavEKF/AP_NavEKF_core_common.h> #include <AP_NavEKF/AP_NavEKF_core_common.h>
#include <AP_NavEKF/AP_NavEKF_Source.h> #include <AP_NavEKF/AP_NavEKF_Source.h>
#include <AP_NavEKF3/AP_NavEKF3_Buffer.h> #include <AP_NavEKF/EKF_Buffer.h>
#include <AP_InertialSensor/AP_InertialSensor.h> #include <AP_InertialSensor/AP_InertialSensor.h>
#include <GCS_MAVLink/GCS_MAVLink.h> #include <GCS_MAVLink/GCS_MAVLink.h>
#include <AP_DAL/AP_DAL.h> #include <AP_DAL/AP_DAL.h>
@ -548,87 +548,75 @@ private:
uint8_t accel_index; uint8_t accel_index;
}; };
struct gps_elements { struct gps_elements : EKF_obs_element_t {
Vector2f pos; // horizontal North East position of the GPS antenna in local NED earth frame (m) Vector2f pos; // horizontal North East position of the GPS antenna in local NED earth frame (m)
float hgt; // height of the GPS antenna in local NED earth frame (m) float hgt; // height of the GPS antenna in local NED earth frame (m)
Vector3f vel; // velocity of the GPS antenna in local NED earth frame (m/sec) Vector3f vel; // velocity of the GPS antenna in local NED earth frame (m/sec)
uint32_t time_ms; // measurement timestamp (msec)
uint8_t sensor_idx; // unique integer identifying the GPS sensor uint8_t sensor_idx; // unique integer identifying the GPS sensor
bool corrected; // true when the position and velocity have been corrected for sensor position bool corrected; // true when the position and velocity have been corrected for sensor position
}; };
struct mag_elements { struct mag_elements : EKF_obs_element_t {
Vector3f mag; // body frame magnetic field measurements (Gauss) Vector3f mag; // body frame magnetic field measurements (Gauss)
uint32_t time_ms; // measurement timestamp (msec)
}; };
struct baro_elements { struct baro_elements : EKF_obs_element_t {
float hgt; // height of the pressure sensor in local NED earth frame (m) float hgt; // height of the pressure sensor in local NED earth frame (m)
uint32_t time_ms; // measurement timestamp (msec)
}; };
struct range_elements { struct range_elements : EKF_obs_element_t {
float rng; // distance measured by the range sensor (m) float rng; // distance measured by the range sensor (m)
uint32_t time_ms; // measurement timestamp (msec)
uint8_t sensor_idx; // integer either 0 or 1 uniquely identifying up to two range sensors uint8_t sensor_idx; // integer either 0 or 1 uniquely identifying up to two range sensors
}; };
struct rng_bcn_elements { struct rng_bcn_elements : EKF_obs_element_t {
float rng; // range measurement to each beacon (m) float rng; // range measurement to each beacon (m)
Vector3f beacon_posNED; // NED position of the beacon (m) Vector3f beacon_posNED; // NED position of the beacon (m)
float rngErr; // range measurement error 1-std (m) float rngErr; // range measurement error 1-std (m)
uint8_t beacon_ID; // beacon identification number uint8_t beacon_ID; // beacon identification number
uint32_t time_ms; // measurement timestamp (msec)
}; };
struct tas_elements { struct tas_elements : EKF_obs_element_t {
float tas; // true airspeed measurement (m/sec) float tas; // true airspeed measurement (m/sec)
uint32_t time_ms; // measurement timestamp (msec)
}; };
struct of_elements { struct of_elements : EKF_obs_element_t {
Vector2f flowRadXY; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec) Vector2f flowRadXY; // raw (non motion compensated) optical flow angular rates about the XY body axes (rad/sec)
Vector2f flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec) Vector2f flowRadXYcomp; // motion compensated XY optical flow angular rates about the XY body axes (rad/sec)
uint32_t time_ms; // measurement timestamp (msec)
Vector3f bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec) Vector3f bodyRadXYZ; // body frame XYZ axis angular rates averaged across the optical flow measurement interval (rad/sec)
Vector3f body_offset; // XYZ position of the optical flow sensor in body frame (m) Vector3f body_offset; // XYZ position of the optical flow sensor in body frame (m)
}; };
struct vel_odm_elements { struct vel_odm_elements : EKF_obs_element_t {
Vector3f vel; // XYZ velocity measured in body frame (m/s) Vector3f vel; // XYZ velocity measured in body frame (m/s)
float velErr; // velocity measurement error 1-std (m/s) float velErr; // velocity measurement error 1-std (m/s)
Vector3f body_offset;// XYZ position of the velocity sensor in body frame (m) Vector3f body_offset;// XYZ position of the velocity sensor in body frame (m)
Vector3f angRate; // angular rate estimated from odometry (rad/sec) Vector3f angRate; // angular rate estimated from odometry (rad/sec)
uint32_t time_ms; // measurement timestamp (msec)
}; };
struct wheel_odm_elements { struct wheel_odm_elements : EKF_obs_element_t {
float delAng; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s) float delAng; // wheel rotation angle measured in body frame - positive is forward movement of vehicle (rad/s)
float radius; // wheel radius (m) float radius; // wheel radius (m)
Vector3f hub_offset; // XYZ position of the wheel hub in body frame (m) Vector3f hub_offset; // XYZ position of the wheel hub in body frame (m)
float delTime; // time interval that the measurement was accumulated over (sec) float delTime; // time interval that the measurement was accumulated over (sec)
uint32_t time_ms; // measurement timestamp (msec)
}; };
struct yaw_elements { struct yaw_elements : EKF_obs_element_t {
float yawAng; // yaw angle measurement (rad) float yawAng; // yaw angle measurement (rad)
float yawAngErr; // yaw angle 1SD measurement accuracy (rad) float yawAngErr; // yaw angle 1SD measurement accuracy (rad)
uint32_t time_ms; // measurement timestamp (msec)
uint8_t type; // type specifiying Euler rotation order used, 1 = 312 (ZXY), 2 = 321 (ZYX) uint8_t type; // type specifiying Euler rotation order used, 1 = 312 (ZXY), 2 = 321 (ZYX)
}; };
struct ext_nav_elements { struct ext_nav_elements : EKF_obs_element_t {
Vector3f pos; // XYZ position measured in a RH navigation frame (m) Vector3f pos; // XYZ position measured in a RH navigation frame (m)
float posErr; // spherical position measurement error 1-std (m) float posErr; // spherical position measurement error 1-std (m)
uint32_t time_ms; // measurement timestamp (msec)
bool posReset; // true when the position measurement has been reset bool posReset; // true when the position measurement has been reset
bool corrected; // true when the position has been corrected for sensor position bool corrected; // true when the position has been corrected for sensor position
}; };
struct ext_nav_vel_elements { struct ext_nav_vel_elements : EKF_obs_element_t {
Vector3f vel; // velocity in NED (m/s) Vector3f vel; // velocity in NED (m/s)
float err; // velocity measurement error (m/s) float err; // velocity measurement error (m/s)
uint32_t time_ms; // measurement timestamp (msec)
bool corrected; // true when the velocity has been corrected for sensor position bool corrected; // true when the velocity has been corrected for sensor position
}; };
@ -1003,13 +991,13 @@ private:
float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts float gpsNoiseScaler; // Used to scale the GPS measurement noise and consistency gates to compensate for operation with small satellite counts
Matrix24 P; // covariance matrix Matrix24 P; // covariance matrix
imu_ring_buffer_t<imu_elements> storedIMU; // IMU data buffer EKF_IMU_buffer_t<imu_elements> storedIMU; // IMU data buffer
obs_ring_buffer_t<gps_elements> storedGPS; // GPS data buffer EKF_obs_buffer_t<gps_elements> storedGPS; // GPS data buffer
obs_ring_buffer_t<mag_elements> storedMag; // Magnetometer data buffer EKF_obs_buffer_t<mag_elements> storedMag; // Magnetometer data buffer
obs_ring_buffer_t<baro_elements> storedBaro; // Baro data buffer EKF_obs_buffer_t<baro_elements> storedBaro; // Baro data buffer
obs_ring_buffer_t<tas_elements> storedTAS; // TAS data buffer EKF_obs_buffer_t<tas_elements> storedTAS; // TAS data buffer
obs_ring_buffer_t<range_elements> storedRange; // Range finder data buffer EKF_obs_buffer_t<range_elements> storedRange; // Range finder data buffer
imu_ring_buffer_t<output_elements> storedOutput;// output state buffer EKF_IMU_buffer_t<output_elements> storedOutput;// output state buffer
Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation Matrix3f prevTnb; // previous nav to body transformation used for INS earth rotation compensation
ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2) ftype accNavMag; // magnitude of navigation accel - used to adjust GPS obs variance (m/s^2)
ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2) ftype accNavMagHoriz; // magnitude of navigation accel in horizontal plane (m/s^2)
@ -1185,7 +1173,7 @@ private:
uint32_t gpsVelInnovTime_ms; // system time that gps velocity innovations were recorded (to detect timeouts) uint32_t gpsVelInnovTime_ms; // system time that gps velocity innovations were recorded (to detect timeouts)
// variables added for optical flow fusion // variables added for optical flow fusion
obs_ring_buffer_t<of_elements> storedOF; // OF data buffer EKF_obs_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
bool flowDataValid; // true while optical flow data is still fresh bool flowDataValid; // true while optical flow data is still fresh
@ -1239,7 +1227,7 @@ private:
bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference bool terrainHgtStable; // true when the terrain height is stable enough to be used as a height reference
// body frame odometry fusion // body frame odometry fusion
obs_ring_buffer_t<vel_odm_elements> storedBodyOdm; // body velocity data buffer EKF_obs_buffer_t<vel_odm_elements> storedBodyOdm; // body velocity data buffer
vel_odm_elements bodyOdmDataNew; // Body frame odometry data at the current time horizon vel_odm_elements bodyOdmDataNew; // Body frame odometry data at the current time horizon
vel_odm_elements bodyOdmDataDelayed; // Body frame odometry data at the fusion time horizon vel_odm_elements bodyOdmDataDelayed; // Body frame odometry data at the fusion time horizon
uint32_t lastbodyVelPassTime_ms; // time stamp when the body velocity measurement last passed innovation consistency checks (msec) uint32_t lastbodyVelPassTime_ms; // time stamp when the body velocity measurement last passed innovation consistency checks (msec)
@ -1252,17 +1240,17 @@ private:
bool bodyVelFusionActive; // true when body frame velocity fusion is active bool bodyVelFusionActive; // true when body frame velocity fusion is active
// wheel sensor fusion // wheel sensor fusion
obs_ring_buffer_t<wheel_odm_elements> storedWheelOdm; // body velocity data buffer EKF_obs_buffer_t<wheel_odm_elements> storedWheelOdm; // body velocity data buffer
wheel_odm_elements wheelOdmDataDelayed; // Body frame odometry data at the fusion time horizon wheel_odm_elements wheelOdmDataDelayed; // Body frame odometry data at the fusion time horizon
// yaw sensor fusion // yaw sensor fusion
uint32_t yawMeasTime_ms; uint32_t yawMeasTime_ms;
obs_ring_buffer_t<yaw_elements> storedYawAng; EKF_obs_buffer_t<yaw_elements> storedYawAng;
yaw_elements yawAngDataNew; yaw_elements yawAngDataNew;
yaw_elements yawAngDataDelayed; yaw_elements yawAngDataDelayed;
// Range Beacon Sensor Fusion // Range Beacon Sensor Fusion
obs_ring_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer EKF_obs_buffer_t<rng_bcn_elements> storedRangeBeacon; // Beacon range buffer
rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon rng_bcn_elements rngBcnDataDelayed; // Range beacon data at the fusion time horizon
uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec) uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innovation consistency checks (msec)
float rngBcnTestRatio; // Innovation test ratio for range beacon measurements float rngBcnTestRatio; // Innovation test ratio for range beacon measurements
@ -1350,13 +1338,13 @@ private:
uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec) uint32_t lastMoveCheckLogTime_ms; // last time the movement check data was logged (msec)
// external navigation fusion // external navigation fusion
obs_ring_buffer_t<ext_nav_elements> storedExtNav; // external navigation data buffer EKF_obs_buffer_t<ext_nav_elements> storedExtNav; // external navigation data buffer
ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon ext_nav_elements extNavDataDelayed; // External nav at the fusion time horizon
uint32_t extNavMeasTime_ms; // time external measurements were accepted for input to the data buffer (msec) uint32_t extNavMeasTime_ms; // time external measurements were accepted for input to the data buffer (msec)
uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec) uint32_t extNavLastPosResetTime_ms; // last time the external nav systen performed a position reset (msec)
bool extNavDataToFuse; // true when there is new external nav data to fuse bool extNavDataToFuse; // true when there is new external nav data to fuse
bool extNavUsedForPos; // true when the external nav data is being used as a position reference. bool extNavUsedForPos; // true when the external nav data is being used as a position reference.
obs_ring_buffer_t<ext_nav_vel_elements> storedExtNavVel; // external navigation velocity data buffer EKF_obs_buffer_t<ext_nav_vel_elements> storedExtNavVel; // external navigation velocity data buffer
ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon. Already corrected for sensor position ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon. Already corrected for sensor position
uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec) uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec)
bool extNavVelToFuse; // true when there is new external navigation velocity to fuse bool extNavVelToFuse; // true when there is new external navigation velocity to fuse