Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_NavEKF2: convert to using common buffer classes

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

199
libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h
View File

@ -1,199 +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;
}
}
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;
};

2
libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp
View File

@ -447,7 +447,7 @@ void NavEKF2_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
// TODO - check if calculations can tolerate 0 // TODO - check if calculations can tolerate 0

69
libraries/AP_NavEKF2/AP_NavEKF2_core.h
View File

@ -28,7 +28,7 @@
#include <AP_Math/AP_Math.h> #include <AP_Math/AP_Math.h>
#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_NavEKF2/AP_NavEKF2_Buffer.h> #include <AP_NavEKF/EKF_Buffer.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>
@ -462,57 +462,49 @@ private:
uint8_t accel_index; uint8_t accel_index;
}; };
struct gps_elements { struct gps_elements : EKF_obs_element_t {
Vector2f pos; // 0..1 Vector2f pos;
float hgt; // 2 float hgt;
Vector3f vel; // 3..5 Vector3f vel;
uint32_t time_ms; // 6 uint8_t sensor_idx;
uint8_t sensor_idx; // 7..9
}; };
struct mag_elements { struct mag_elements : EKF_obs_element_t {
Vector3f mag; // 0..2 Vector3f mag;
uint32_t time_ms; // 3
}; };
struct baro_elements { struct baro_elements : EKF_obs_element_t {
float hgt; // 0 float hgt;
uint32_t time_ms; // 1
}; };
struct range_elements { struct range_elements : EKF_obs_element_t {
float rng; // 0 float rng;
uint32_t time_ms; // 1 uint8_t sensor_idx;
uint8_t sensor_idx; // 2
}; };
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; // 0 float tas;
uint32_t time_ms; // 1
}; };
struct of_elements { struct of_elements : EKF_obs_element_t {
Vector2f flowRadXY; Vector2f flowRadXY;
Vector2f flowRadXYcomp; Vector2f flowRadXYcomp;
uint32_t time_ms;
Vector3f bodyRadXYZ; Vector3f bodyRadXYZ;
Vector3f body_offset; Vector3f body_offset;
}; };
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)
Quaternion quat; // quaternion describing the rotation from navigation to body frame Quaternion quat; // quaternion describing the rotation from navigation to body frame
float posErr; // spherical poition measurement error 1-std (m) float posErr; // spherical poition measurement error 1-std (m)
float angErr; // spherical angular measurement error 1-std (rad) float angErr; // spherical angular measurement error 1-std (rad)
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
}; };
@ -524,10 +516,9 @@ private:
float accel_zbias; float accel_zbias;
} inactiveBias[INS_MAX_INSTANCES]; } inactiveBias[INS_MAX_INSTANCES];
struct ext_nav_vel_elements { struct ext_nav_vel_elements : EKF_obs_element_t {
Vector3f vel; // velocity in NED (m) Vector3f vel; // velocity in NED (m)
float err; // velocity measurement error (m/s) float err; // velocity measurement error (m/s)
uint32_t time_ms; // measurement timestamp (msec)
}; };
// update the navigation filter status // update the navigation filter status
@ -841,13 +832,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)
@ -1015,7 +1006,7 @@ private:
bool gpsAccuracyGood; // true when the GPS accuracy is considered to be good enough for safe flight. bool gpsAccuracyGood; // true when the GPS accuracy is considered to be good enough for safe flight.
// 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 flowDataToFuse; // true when optical flow data is ready for fusion bool flowDataToFuse; // true when optical flow data is ready for fusion
@ -1069,7 +1060,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
// 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 rngBcnDataNew; // Range beacon data at the current time horizon rng_bcn_elements rngBcnDataNew; // Range beacon data at the current time horizon
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 innvovation consistency checks (msec) uint32_t lastRngBcnPassTime_ms; // time stamp when the range beacon measurement last passed innvovation consistency checks (msec)
@ -1143,7 +1134,7 @@ private:
uint8_t magYawAnomallyCount; // Number of times the yaw has been reset due to a magnetic anomaly during initial ascent uint8_t magYawAnomallyCount; // Number of times the yaw has been reset due to a magnetic anomaly during initial ascent
// 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 extNavDataNew; // External nav data at the current time horizon ext_nav_elements extNavDataNew; // External nav data at the current time horizon
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)
@ -1154,7 +1145,7 @@ private:
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.
bool extNavYawResetRequest; // true when a reset of vehicle yaw using the external nav data is requested bool extNavYawResetRequest; // true when a reset of vehicle yaw using the external nav data is requested
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 extNavVelNew; // external navigation velocity data at the current time horizon ext_nav_vel_elements extNavVelNew; // external navigation velocity data at the current time horizon
ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon ext_nav_vel_elements extNavVelDelayed; // external navigation velocity data at the fusion time horizon
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)