Line data    Source code

       1             : /*
       2             :   common EKF Buffer class. This handles the storage buffers for EKF data to
       3             :   bring it onto the fusion time horizon
       4             : */
       5             : 
       6             : #include "EKF_Buffer.h"
       7             : #include <stdlib.h>
       8             : #include <string.h>
       9             : #include <AP_InternalError/AP_InternalError.h>
      10             : 
      11             : // constructor
      12       65046 : ekf_ring_buffer::ekf_ring_buffer(uint8_t _elsize) :
      13       65046 :     elsize(_elsize),
      14       65046 :     buffer(nullptr)
      15       65046 : {}
      16             : 
      17       26849 : bool ekf_ring_buffer::init(uint8_t _size)
      18             : {
      19       26849 :     if (buffer) {
      20           0 :         free(buffer);
      21             :     }
      22       26849 :     buffer = calloc(_size, elsize);
      23       26849 :     if (buffer == nullptr) {
      24           0 :         return false;
      25             :     }
      26       26849 :     size = _size;
      27       26849 :     reset();
      28       26849 :     return true;
      29             : }
      30             : 
      31             : /*
      32             :   get buffer offset for an index
      33             :  */
      34    58469336 : void *ekf_ring_buffer::get_offset(uint8_t idx) const
      35             : {
      36    58469336 :     return (void*)(((uint8_t*)buffer)+idx*uint32_t(elsize));
      37             : }
      38             : 
      39             : /*
      40             :   get a reference to the timestamp for an index
      41             :  */
      42    38029521 : uint32_t ekf_ring_buffer::time_ms(uint8_t idx) const
      43             : {
      44    38029521 :     EKF_obs_element_t *el = (EKF_obs_element_t *)get_offset(idx);
      45    38029521 :     return el->time_ms;
      46             : }
      47             : 
      48             : /*
      49             :   Search through a ring buffer and return the newest data that is
      50             :   older than the time specified by sample_time_ms
      51             :   Returns false if no data can be found that is less than 100msec old
      52             : */
      53   221523785 : bool ekf_ring_buffer::recall(void *element, const uint32_t sample_time_ms)
      54             : {
      55   221523785 :     bool ret = false;
      56   221523785 :     uint8_t best_index = 0;  // only valid when ret becomes true
      57   231750519 :     while (count > 0) {
      58    38029521 :         const uint32_t toldest = time_ms(oldest);
      59    38029521 :         const int32_t dt = sample_time_ms - toldest;
      60    38029521 :         const bool matches = dt >= 0 && dt < 100;
      61    38029521 :         if (matches) {
      62    10221909 :             best_index = oldest;
      63    10221909 :             ret = true;
      64             :         }
      65    38029521 :         if (dt < 0) {
      66             :             // the oldest element is younger than we want, stop
      67             :             // searching and don't consume this element
      68    27802787 :             break;
      69             :         }
      70             :         // discard the sample
      71    10226734 :         count--;
      72    10226734 :         oldest = (oldest+1) % size;
      73             :     }
      74             : 
      75   221523785 :     if (ret) {
      76    10164084 :         memcpy(element, get_offset(best_index), elsize);
      77             :     }
      78             : 
      79   221523785 :     return ret;
      80             : }
      81             : 
      82             : /*
      83             :  * Writes data and timestamp to a Ring buffer and advances indices that
      84             :  * define the location of the newest and oldest data
      85             :  */
      86    10281491 : void ekf_ring_buffer::push(const void *element)
      87             : {
      88    10281491 :     if (buffer == nullptr) {
      89        5760 :         return;
      90             :     }
      91             : 
      92             :     // Advance head to next available index
      93    10275731 :     const uint8_t head = (oldest+count) % size;
      94             : 
      95             :     // New data is written at the head
      96    10275731 :     memcpy(get_offset(head), element, elsize);
      97             : 
      98    10275731 :     if (count < size) {
      99    10246247 :         count++;
     100             :     } else {
     101       29484 :         oldest = (oldest+1) % size;
     102             :     }
     103             : }
     104             : 
     105             : 
     106             : // zeroes all data in the ring buffer
     107       74444 : void ekf_ring_buffer::reset()
     108             : {
     109       74444 :     count = 0;
     110       74444 :     oldest = 0;
     111       74444 : }
     112             : 
     113             : ////////////////////////////////////////////////////
     114             : /*
     115             :   IMU buffer operations implemented separately due to different
     116             :   semantics
     117             : */
     118             : 
     119             : // constructor
     120       10026 : ekf_imu_buffer::ekf_imu_buffer(uint8_t _elsize) :
     121       10026 :     elsize(_elsize)
     122       10026 : {}
     123             : 
     124             : /*
     125             :   get buffer offset for an index
     126             :  */
     127   572944365 : void *ekf_imu_buffer::get_offset(uint8_t idx) const
     128             : {
     129   572944365 :     return (void*)(((uint8_t*)buffer)+idx*uint32_t(elsize));
     130             : }
     131             : 
     132             : // initialise buffer, returns false when allocation has failed
     133        9958 : bool ekf_imu_buffer::init(uint32_t size)
     134             : {
     135        9958 :     if (buffer != nullptr) {
     136             :         // allow for init twice
     137           0 :         free(buffer);
     138             :     }
     139        9958 :     buffer = calloc(size, elsize);
     140        9958 :     if (buffer == nullptr) {
     141           0 :         return false;
     142             :     }
     143        9958 :     _size = size;
     144        9958 :     _youngest = 0;
     145        9958 :     _oldest = 0;
     146        9958 :     _filled = false;
     147        9958 :     return true;
     148             : }
     149             : 
     150             : /*
     151             :   Writes data to a Ring buffer and advances indices that
     152             :   define the location of the newest and oldest data
     153             : */
     154    23335119 : void ekf_imu_buffer::push_youngest_element(const void *element)
     155             : {
     156    23335119 :     if (!buffer) {
     157           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     158           0 :         return;
     159             :     }
     160             :     // push youngest to the buffer
     161    23335119 :     _youngest++;
     162    23335119 :     if (_youngest == _size) {
     163     2466892 :         _youngest = 0;
     164     2466892 :         _filled = true;
     165             :     }
     166    23335119 :     memcpy(get_offset(_youngest), element, elsize);
     167             :     // set oldest data index
     168    23335119 :     _oldest = (_youngest+1) % _size;
     169             : }
     170             : 
     171             : // retrieve the oldest data from the ring buffer tail
     172    23335111 : void ekf_imu_buffer::get_oldest_element(void *element)
     173             : {
     174    23335111 :     if (buffer == nullptr) {
     175           0 :         INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
     176           0 :         memset(element, 0, elsize);
     177             :     } else {
     178    23335111 :         memcpy(element, get_offset(_oldest), elsize);
     179             :     }
     180    23335111 : }
     181             : 
     182             : // writes the same data to all elements in the ring buffer
     183         558 : void ekf_imu_buffer::reset_history(const void *element)
     184             : {
     185        8514 :     for (uint8_t index=0; index<_size; index++) {
     186        7956 :         memcpy(get_offset(index), element, elsize);
     187             :     }
     188         558 : }
     189             : 
     190             : // zeroes all data in the ring buffer
     191        7406 : void ekf_imu_buffer::reset()
     192             : {
     193        7406 :     _youngest = 0;
     194        7406 :     _oldest = 0;
     195        7406 :     memset(buffer, 0, _size*uint32_t(elsize));
     196        7406 : }
     197             : 
     198             : // retrieves data from the ring buffer at a specified index
     199   526266179 : void *ekf_imu_buffer::get(uint8_t index) const
     200             : {
     201   526266179 :     return get_offset(index);
     202             : }