Line data    Source code

       1             : /*
       2             :   common EKF buffer classes. These handles the storage buffers for EKF
       3             :   data to bring it onto the fusion time horizon
       4             : */
       5             : 
       6             : #include <stdint.h>
       7             : #include <type_traits>
       8             : 
       9             : typedef struct {
      10             :     // measurement timestamp (msec)
      11             :     uint32_t    time_ms;
      12             : } EKF_obs_element_t;
      13             : 
      14             : // this class is to be used for observation buffers, the data is
      15             : // pushed into buffer like any standard ring buffer return is based on
      16             : // the sample time provided
      17             : class ekf_ring_buffer
      18             : {
      19             : public:
      20             :     ekf_ring_buffer(uint8_t elsize);
      21             : 
      22             :     // initialise buffer, returns false when allocation has failed
      23             :     bool init(uint8_t size);
      24             : 
      25             :     /*
      26             :      * Searches through a ring buffer and return the newest data that is older than the
      27             :      * time specified by sample_time_ms
      28             :      * Zeros old data so it cannot not be used again
      29             :      * Returns false if no data can be found that is less than 100msec old
      30             :     */
      31             :     bool recall(void *element, const uint32_t sample_time_ms);
      32             : 
      33             :     /*
      34             :      * Writes data and timestamp to a Ring buffer and advances indices that
      35             :      * define the location of the newest and oldest data
      36             :     */
      37             :     void push(const void *element);
      38             : 
      39             :     // zeroes all data in the ring buffer
      40             :     void reset();
      41             : 
      42             : private:
      43             :     const uint8_t elsize;
      44             :     void *buffer;
      45             : 
      46             :     // size of allocated buffer in elsize units
      47             :     uint8_t size;
      48             : 
      49             :     // index of the oldest element in the buffer
      50             :     uint8_t oldest;
      51             : 
      52             :     // total number of elements in the buffer
      53             :     uint8_t count;
      54             : 
      55             :     uint32_t time_ms(uint8_t idx) const;
      56             :     void *get_offset(uint8_t idx) const;
      57             : };
      58             : 
      59             : /*
      60             :   template class for more convenient type handling
      61             :  */
      62             : template <typename element_type>
      63             : class EKF_obs_buffer_t : ekf_ring_buffer
      64             : {
      65             :     static_assert(
      66             :         std::is_base_of<EKF_obs_element_t, element_type>::value,
      67             :         "must be a descendant of EKF_obs_element_t"
      68             :     );
      69             : public:
      70       65046 :     EKF_obs_buffer_t() :
      71       65046 :         ekf_ring_buffer(sizeof(element_type))
      72       65046 :         {}
      73             : 
      74       26849 :     bool init(uint8_t _size) {
      75       26849 :         return ekf_ring_buffer::init(_size);
      76             :     }
      77             : 
      78   221523785 :     bool recall(element_type &element,uint32_t sample_time) {
      79   221523785 :         return ekf_ring_buffer::recall(&element, sample_time);
      80             :     }
      81             : 
      82    10281491 :     void push(const element_type &element) {
      83    10281491 :         return ekf_ring_buffer::push(&element);
      84             :     }
      85             : 
      86       47595 :     void reset() {
      87       47595 :         return ekf_ring_buffer::reset();
      88             :     }
      89             : };
      90             : 
      91             : 
      92             : /*
      93             :   ring buffer for IMU data,
      94             : */
      95             : class ekf_imu_buffer
      96             : {
      97             : public:
      98             :     ekf_imu_buffer(uint8_t elsize);
      99             :     
     100             :     // initialise buffer, returns false when allocation has failed
     101             :     bool init(uint32_t size);
     102             : 
     103             :     /*
     104             :       Writes data to a Ring buffer and advances indices that
     105             :       define the location of the newest and oldest data
     106             :     */
     107             :     void push_youngest_element(const void *element);
     108             : 
     109             :     // return true if the buffer has been filled at least once
     110       20163 :     bool is_filled(void) const {
     111       20163 :         return _filled;
     112             :     }
     113             :     
     114             :     // retrieve the oldest data from the ring buffer tail
     115             :     void get_oldest_element(void *element);
     116             : 
     117             :     // writes the same data to all elements in the ring buffer
     118             :     void reset_history(const void *element);
     119             : 
     120             :     // zeroes all data in the ring buffer
     121             :     void reset();
     122             : 
     123             :     // retrieves data from the ring buffer at a specified index
     124             :     void *get(uint8_t index) const;
     125             : 
     126             :     // returns the index for the ring buffer oldest data
     127    23213820 :     uint8_t get_oldest_index() const {
     128    23213820 :         return _oldest;
     129             :     }
     130             : 
     131             :     // returns the index for the ring buffer youngest data
     132    46427634 :     uint8_t get_youngest_index() const {
     133    46427634 :         return _youngest;
     134             :     }
     135             : 
     136             : protected:
     137             :     const uint8_t elsize;
     138             :     void *buffer;
     139             :     uint8_t _size,_oldest,_youngest;
     140             :     bool _filled;
     141             : 
     142             :     void *get_offset(uint8_t idx) const;
     143             : };
     144             : 
     145             : /*
     146             :   template class for more convenient type handling
     147             :  */
     148             : template <typename element_type>
     149             : class EKF_IMU_buffer_t : ekf_imu_buffer
     150             : {
     151             : public:
     152       10022 :     EKF_IMU_buffer_t() :
     153       10022 :         ekf_imu_buffer(sizeof(element_type))
     154       10022 :         {}
     155             : 
     156        9954 :     bool init(uint8_t size) {
     157        9954 :         return ekf_imu_buffer::init(size);
     158             :     }
     159             : 
     160             :     /*
     161             :       Writes data to a Ring buffer and advances indices that
     162             :      define the location of the newest and oldest data
     163             :     */
     164    23335109 :     void push_youngest_element(element_type element) {
     165    23335109 :         return ekf_imu_buffer::push_youngest_element(&element);
     166             :     }
     167             : 
     168             :     // return true if the buffer has been filled at least once
     169       20149 :     bool is_filled(void) const {
     170       20149 :         return ekf_imu_buffer::is_filled();
     171             :     }
     172             :     
     173             :     // retrieve the oldest data from the ring buffer tail
     174    23335109 :     element_type get_oldest_element() {
     175    23335109 :         element_type ret;
     176    23335109 :         ekf_imu_buffer::get_oldest_element(&ret);
     177    23335109 :         return ret;
     178             :     }
     179             : 
     180             :     // writes the same data to all elements in the ring buffer
     181         558 :     void reset_history(element_type element) {
     182         558 :         ekf_imu_buffer::reset_history(&element);
     183         558 :     }
     184             : 
     185             :     // zeroes all data in the ring buffer
     186        7404 :     void reset() {
     187        7404 :         ekf_imu_buffer::reset();
     188        7404 :     }
     189             : 
     190             :     // retrieves data from the ring buffer at a specified index
     191   526266177 :     element_type& operator[](uint32_t index) {
     192   526266177 :         element_type *ret = (element_type *)ekf_imu_buffer::get(index);
     193   526266177 :         return *ret;
     194             :     }
     195             : 
     196             :     // returns the index for the ring buffer oldest data
     197    23213814 :     uint8_t get_oldest_index() {
     198    23213814 :         return ekf_imu_buffer::get_oldest_index();
     199             :     }
     200             : 
     201             :     // returns the index for the ring buffer youngest data
     202    46427628 :     inline uint8_t get_youngest_index() {
     203    46427628 :         return ekf_imu_buffer::get_youngest_index();
     204             :     }
     205             : };