APM:Libraries
AP_NavEKF3_Buffer.h
Go to the documentation of this file.
1 // EKF Buffer models
2 
3 // this buffer model is to be used for observation buffers,
4 // the data is pushed into buffer like any standard ring buffer
5 // return is based on the sample time provided
6 template <typename element_type>
8 {
9 public:
10  struct element_t{
11  element_type element;
12  } *buffer;
13 
14  // initialise buffer, returns false when allocation has failed
15  bool init(uint32_t size)
16  {
17  buffer = new element_t[size];
18  if(buffer == nullptr)
19  {
20  return false;
21  }
22  memset(buffer,0,size*sizeof(element_t));
23  _size = size;
24  _head = 0;
25  _tail = 0;
26  _new_data = false;
27  return true;
28  }
29 
30  /*
31  * Searches through a ring buffer and return the newest data that is older than the
32  * time specified by sample_time_ms
33  * Zeros old data so it cannot not be used again
34  * Returns false if no data can be found that is less than 100msec old
35  */
36 
37  bool recall(element_type &element,uint32_t sample_time)
38  {
39  if(!_new_data) {
40  return false;
41  }
42  bool success = false;
43  uint8_t tail = _tail, bestIndex;
44 
45  if(_head == tail) {
46  if (buffer[tail].element.time_ms != 0 && buffer[tail].element.time_ms <= sample_time) {
47  // if head is equal to tail just check if the data is unused and within time horizon window
48  if (((sample_time - buffer[tail].element.time_ms) < 100)) {
49  bestIndex = tail;
50  success = true;
51  _new_data = false;
52  }
53  }
54  } else {
55  while(_head != tail) {
56  // find a measurement older than the fusion time horizon that we haven't checked before
57  if (buffer[tail].element.time_ms != 0 && buffer[tail].element.time_ms <= sample_time) {
58  // Find the most recent non-stale measurement that meets the time horizon criteria
59  if (((sample_time - buffer[tail].element.time_ms) < 100)) {
60  bestIndex = tail;
61  success = true;
62  }
63  } else if(buffer[tail].element.time_ms > sample_time){
64  break;
65  }
66  tail = (tail+1)%_size;
67  }
68  }
69 
70  if (success) {
71  element = buffer[bestIndex].element;
72  _tail = (bestIndex+1)%_size;
73  //make time zero to stop using it again,
74  //resolves corner case of reusing the element when head == tail
75  buffer[bestIndex].element.time_ms = 0;
76  return true;
77  } else {
78  return false;
79  }
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  inline void push(element_type element)
87  {
88  // Advance head to next available index
89  _head = (_head+1)%_size;
90  // New data is written at the head
92  _new_data = true;
93  }
94  // writes the same data to all elements in the ring buffer
95  inline void reset_history(element_type element, uint32_t sample_time) {
96  for (uint8_t index=0; index<_size; index++) {
97  buffer[index].element = element;
98  }
99  }
100 
101  // zeroes all data in the ring buffer
102  inline void reset() {
103  _head = 0;
104  _tail = 0;
105  _new_data = false;
106  memset(buffer,0,_size*sizeof(element_t));
107  }
108 
109 private:
110  uint8_t _size,_head,_tail,_new_data;
111 };
112 
113 
114 // Following buffer model is for IMU data,
115 // it achieves a distance of sample size
116 // between youngest and oldest
117 template <typename element_type>
118 class imu_ring_buffer_t
119 {
120 public:
121  struct element_t{
122  element_type element;
123  } *buffer;
124 
125  // initialise buffer, returns false when allocation has failed
126  bool init(uint32_t size)
127  {
128  buffer = new element_t[size];
129  if(buffer == nullptr)
130  {
131  return false;
132  }
133  memset(buffer,0,size*sizeof(element_t));
134  _size = size;
135  _youngest = 0;
136  _oldest = 0;
137  return true;
138  }
139  /*
140  * Writes data to a Ring buffer and advances indices that
141  * define the location of the newest and oldest data
142  */
143  inline void push_youngest_element(element_type element)
144  {
145  // push youngest to the buffer
146  _youngest = (_youngest+1)%_size;
147  buffer[_youngest].element = element;
148  // set oldest data index
149  _oldest = (_youngest+1)%_size;
150  if (_oldest == 0) {
151  _filled = true;
152  }
153  }
154 
155  // return true if the buffer has been filled at least once
156  inline bool is_filled(void) const {
157  return _filled;
158  }
159 
160  // retrieve the oldest data from the ring buffer tail
161  inline element_type pop_oldest_element() {
162  element_type ret = buffer[_oldest].element;
163  return ret;
164  }
165 
166  // writes the same data to all elements in the ring buffer
167  inline void reset_history(element_type element) {
168  for (uint8_t index=0; index<_size; index++) {
169  buffer[index].element = element;
170  }
171  }
172 
173  // zeroes all data in the ring buffer
174  inline void reset() {
175  _youngest = 0;
176  _oldest = 0;
177  memset(buffer,0,_size*sizeof(element_t));
178  }
179 
180  // retrieves data from the ring buffer at a specified index
181  inline element_type& operator[](uint32_t index) {
182  return buffer[index].element;
183  }
184 
185  // returns the index for the ring buffer oldest data
186  inline uint8_t get_oldest_index(){
187  return _oldest;
188  }
189 
190  // returns the index for the ring buffer youngest data
191  inline uint8_t get_youngest_index(){
192  return _youngest;
193  }
194 private:
195  uint8_t _size,_oldest,_youngest;
196  bool _filled;
197 };
bool recall(element_type &element, uint32_t sample_time)
void push_youngest_element(element_type element)
bool init(uint32_t size)
void reset_history(element_type element)
uint8_t get_youngest_index()
void push(element_type element)
struct obs_ring_buffer_t::element_t * buffer
bool init(uint32_t size)
element_type & operator[](uint32_t index)
element_type pop_oldest_element()
void reset_history(element_type element, uint32_t sample_time)
bool is_filled(void) const