From 39ba8a8c83c253b9a3d7f7507323e901b99c9e81 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Nov 2020 16:11:21 +1100 Subject: [PATCH] AP_NavEKF2: convert to using common buffer classes this saves a considerable amount of flash --- libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h | 199 ------------------ .../AP_NavEKF2/AP_NavEKF2_Measurements.cpp | 2 +- libraries/AP_NavEKF2/AP_NavEKF2_core.h | 69 +++--- 3 files changed, 31 insertions(+), 239 deletions(-) delete mode 100644 libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h b/libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h deleted file mode 100644 index ae35dca04c..0000000000 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Buffer.h +++ /dev/null @@ -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 -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 -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; -}; diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp index 2f309519e7..96f76fd4a1 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp +++ b/libraries/AP_NavEKF2/AP_NavEKF2_Measurements.cpp @@ -447,7 +447,7 @@ void NavEKF2_core::readIMUData() runUpdates = true; // 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 // TODO - check if calculations can tolerate 0 diff --git a/libraries/AP_NavEKF2/AP_NavEKF2_core.h b/libraries/AP_NavEKF2/AP_NavEKF2_core.h index b11084b019..6dbcfcb54f 100644 --- a/libraries/AP_NavEKF2/AP_NavEKF2_core.h +++ b/libraries/AP_NavEKF2/AP_NavEKF2_core.h @@ -28,7 +28,7 @@ #include #include #include -#include +#include #include #include @@ -462,57 +462,49 @@ private: uint8_t accel_index; }; - struct gps_elements { - Vector2f pos; // 0..1 - float hgt; // 2 - Vector3f vel; // 3..5 - uint32_t time_ms; // 6 - uint8_t sensor_idx; // 7..9 + struct gps_elements : EKF_obs_element_t { + Vector2f pos; + float hgt; + Vector3f vel; + uint8_t sensor_idx; }; - struct mag_elements { - Vector3f mag; // 0..2 - uint32_t time_ms; // 3 + struct mag_elements : EKF_obs_element_t { + Vector3f mag; }; - struct baro_elements { - float hgt; // 0 - uint32_t time_ms; // 1 + struct baro_elements : EKF_obs_element_t { + float hgt; }; - struct range_elements { - float rng; // 0 - uint32_t time_ms; // 1 - uint8_t sensor_idx; // 2 + struct range_elements : EKF_obs_element_t { + float rng; + uint8_t sensor_idx; }; - struct rng_bcn_elements { + struct rng_bcn_elements : EKF_obs_element_t { float rng; // range measurement to each beacon (m) Vector3f beacon_posNED; // NED position of the beacon (m) float rngErr; // range measurement error 1-std (m) uint8_t beacon_ID; // beacon identification number - uint32_t time_ms; // measurement timestamp (msec) }; - struct tas_elements { - float tas; // 0 - uint32_t time_ms; // 1 + struct tas_elements : EKF_obs_element_t { + float tas; }; - struct of_elements { + struct of_elements : EKF_obs_element_t { Vector2f flowRadXY; Vector2f flowRadXYcomp; - uint32_t time_ms; Vector3f bodyRadXYZ; 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) Quaternion quat; // quaternion describing the rotation from navigation to body frame float posErr; // spherical poition measurement error 1-std (m) 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 }; @@ -524,10 +516,9 @@ private: float accel_zbias; } inactiveBias[INS_MAX_INSTANCES]; - struct ext_nav_vel_elements { + struct ext_nav_vel_elements : EKF_obs_element_t { Vector3f vel; // velocity in NED (m) float err; // velocity measurement error (m/s) - uint32_t time_ms; // measurement timestamp (msec) }; // 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 Matrix24 P; // covariance matrix - imu_ring_buffer_t storedIMU; // IMU data buffer - obs_ring_buffer_t storedGPS; // GPS data buffer - obs_ring_buffer_t storedMag; // Magnetometer data buffer - obs_ring_buffer_t storedBaro; // Baro data buffer - obs_ring_buffer_t storedTAS; // TAS data buffer - obs_ring_buffer_t storedRange; // Range finder data buffer - imu_ring_buffer_t storedOutput;// output state buffer + EKF_IMU_buffer_t storedIMU; // IMU data buffer + EKF_obs_buffer_t storedGPS; // GPS data buffer + EKF_obs_buffer_t storedMag; // Magnetometer data buffer + EKF_obs_buffer_t storedBaro; // Baro data buffer + EKF_obs_buffer_t storedTAS; // TAS data buffer + EKF_obs_buffer_t storedRange; // Range finder data buffer + EKF_IMU_buffer_t storedOutput;// output state buffer 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 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. // variables added for optical flow fusion - obs_ring_buffer_t storedOF; // OF data buffer + EKF_obs_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 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 // Range Beacon Sensor Fusion - obs_ring_buffer_t storedRangeBeacon; // Beacon range buffer + EKF_obs_buffer_t storedRangeBeacon; // Beacon range buffer rng_bcn_elements rngBcnDataNew; // Range beacon data at the current 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) @@ -1143,7 +1134,7 @@ private: uint8_t magYawAnomallyCount; // Number of times the yaw has been reset due to a magnetic anomaly during initial ascent // external navigation fusion - obs_ring_buffer_t storedExtNav; // external navigation data buffer + EKF_obs_buffer_t storedExtNav; // external navigation data buffer ext_nav_elements extNavDataNew; // External nav data at the current 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) @@ -1154,7 +1145,7 @@ private: 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 - obs_ring_buffer_t storedExtNavVel; // external navigation velocity data buffer + EKF_obs_buffer_t storedExtNavVel; // external navigation velocity data buffer 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 uint32_t extNavVelMeasTime_ms; // time external navigation velocity measurements were accepted for input to the data buffer (msec)