Skip to content

Commit e4a9497

Browse files
tridgetridge
committed
AP_NavEKF: added common EKF buffer classes
1 parent b1594f9 commit e4a9497

File tree

2 files changed

+396
-0
lines changed

2 files changed

+396
-0
lines changed

‎libraries/AP_NavEKF/EKF_Buffer.cpp

Lines changed: 199 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,199 @@
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+
10+
// constructor
11+
ekf_ring_buffer::ekf_ring_buffer(uint8_t _elsize) :
12+
elsize(_elsize)
13+
{}
14+
15+
bool ekf_ring_buffer::init(uint8_t size)
16+
{
17+
buffer = calloc(size, elsize);
18+
if (buffer == nullptr) {
19+
return false;
20+
}
21+
_size = size;
22+
_head = 0;
23+
_tail = 0;
24+
_new_data = false;
25+
return true;
26+
}
27+
28+
/*
29+
get buffer offset for an index
30+
*/
31+
void *ekf_ring_buffer::get_offset(uint8_t idx) const
32+
{
33+
return (void*)(((uint8_t*)buffer)+idx*uint32_t(elsize));
34+
}
35+
36+
/*
37+
get a reference to the timestamp for an index
38+
*/
39+
uint32_t &ekf_ring_buffer::time_ms(uint8_t idx)
40+
{
41+
EKF_obs_element_t *el = (EKF_obs_element_t *)get_offset(idx);
42+
return el->time_ms;
43+
}
44+
45+
/*
46+
Search through a ring buffer and return the newest data that is
47+
older than the time specified by sample_time_ms Zeros old data
48+
so it cannot not be used again Returns false if no data can be
49+
found that is less than 100msec old
50+
*/
51+
bool ekf_ring_buffer::recall(void *element,uint32_t sample_time)
52+
{
53+
if (!_new_data) {
54+
return false;
55+
}
56+
bool success = false;
57+
uint8_t tail = _tail, bestIndex;
58+
59+
if (_head == tail) {
60+
if (time_ms(tail) != 0 && time_ms(tail) <= sample_time) {
61+
// if head is equal to tail just check if the data is unused and within time horizon window
62+
if (((sample_time - time_ms(tail)) < 100)) {
63+
bestIndex = tail;
64+
success = true;
65+
_new_data = false;
66+
}
67+
}
68+
} else {
69+
while(_head != tail) {
70+
// find a measurement older than the fusion time horizon that we haven't checked before
71+
if (time_ms(tail) != 0 && time_ms(tail) <= sample_time) {
72+
// Find the most recent non-stale measurement that meets the time horizon criteria
73+
if (((sample_time - time_ms(tail)) < 100)) {
74+
bestIndex = tail;
75+
success = true;
76+
}
77+
} else if (time_ms(tail) > sample_time){
78+
break;
79+
}
80+
tail = (tail+1) % _size;
81+
}
82+
}
83+
84+
if (!success) {
85+
return false;
86+
}
87+
88+
memcpy(element, get_offset(bestIndex), elsize);
89+
_tail = (bestIndex+1) % _size;
90+
// make time zero to stop using it again,
91+
// resolves corner case of reusing the element when head == tail
92+
time_ms(bestIndex) = 0;
93+
return true;
94+
}
95+
96+
/*
97+
* Writes data and timestamp to a Ring buffer and advances indices that
98+
* define the location of the newest and oldest data
99+
*/
100+
void ekf_ring_buffer::push(const void *element)
101+
{
102+
if (buffer == nullptr) {
103+
return;
104+
}
105+
// Advance head to next available index
106+
_head = (_head+1) % _size;
107+
// New data is written at the head
108+
memcpy(get_offset(_head), element, elsize);
109+
_new_data = true;
110+
}
111+
112+
113+
// zeroes all data in the ring buffer
114+
void ekf_ring_buffer::reset()
115+
{
116+
_head = 0;
117+
_tail = 0;
118+
_new_data = false;
119+
memset((void *)buffer,0,_size*uint32_t(elsize));
120+
}
121+
122+
////////////////////////////////////////////////////
123+
/*
124+
IMU buffer operations implemented separately due to different
125+
semantics
126+
*/
127+
128+
// constructor
129+
ekf_imu_buffer::ekf_imu_buffer(uint8_t _elsize) :
130+
elsize(_elsize)
131+
{}
132+
133+
/*
134+
get buffer offset for an index
135+
*/
136+
void *ekf_imu_buffer::get_offset(uint8_t idx) const
137+
{
138+
return (void*)(((uint8_t*)buffer)+idx*uint32_t(elsize));
139+
}
140+
141+
// initialise buffer, returns false when allocation has failed
142+
bool ekf_imu_buffer::init(uint32_t size)
143+
{
144+
buffer = calloc(size, elsize);
145+
if (buffer == nullptr) {
146+
return false;
147+
}
148+
_size = size;
149+
_youngest = 0;
150+
_oldest = 0;
151+
return true;
152+
}
153+
154+
/*
155+
Writes data to a Ring buffer and advances indices that
156+
define the location of the newest and oldest data
157+
*/
158+
void ekf_imu_buffer::push_youngest_element(const void *element)
159+
{
160+
if (!buffer) {
161+
return;
162+
}
163+
// push youngest to the buffer
164+
_youngest = (_youngest+1) % _size;
165+
memcpy(get_offset(_youngest), element, elsize);
166+
// set oldest data index
167+
_oldest = (_youngest+1) % _size;
168+
if (_oldest == 0) {
169+
_filled = true;
170+
}
171+
}
172+
173+
// retrieve the oldest data from the ring buffer tail
174+
void ekf_imu_buffer::get_oldest_element(void *element)
175+
{
176+
memcpy(element, get_offset(_oldest), elsize);
177+
}
178+
179+
// writes the same data to all elements in the ring buffer
180+
void ekf_imu_buffer::reset_history(const void *element)
181+
{
182+
for (uint8_t index=0; index<_size; index++) {
183+
memcpy(get_offset(index), element, elsize);
184+
}
185+
}
186+
187+
// zeroes all data in the ring buffer
188+
void ekf_imu_buffer::reset()
189+
{
190+
_youngest = 0;
191+
_oldest = 0;
192+
memset(buffer, 0, _size*uint32_t(elsize));
193+
}
194+
195+
// retrieves data from the ring buffer at a specified index
196+
void *ekf_imu_buffer::get(uint8_t index) const
197+
{
198+
return get_offset(index);
199+
}

‎libraries/AP_NavEKF/EKF_Buffer.h

Lines changed: 197 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,197 @@
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, uint32_t sample_time);
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+
uint8_t _size, _head, _tail, _new_data;
46+
47+
uint32_t &time_ms(uint8_t idx);
48+
void *get_offset(uint8_t idx) const;
49+
};
50+
51+
/*
52+
template class for more convenient type handling
53+
*/
54+
template <typename element_type>
55+
class EKF_obs_buffer_t : ekf_ring_buffer
56+
{
57+
static_assert(
58+
std::is_base_of<EKF_obs_element_t, element_type>::value,
59+
"must be a descendant of EKF_obs_element_t"
60+
);
61+
public:
62+
EKF_obs_buffer_t() :
63+
ekf_ring_buffer(sizeof(element_type))
64+
{}
65+
66+
bool init(uint8_t size) {
67+
return ekf_ring_buffer::init(size);
68+
}
69+
70+
bool recall(element_type &element,uint32_t sample_time) {
71+
return ekf_ring_buffer::recall(&element, sample_time);
72+
}
73+
74+
void push(element_type element) {
75+
return ekf_ring_buffer::push(&element);
76+
}
77+
78+
void reset() {
79+
return ekf_ring_buffer::reset();
80+
}
81+
};
82+
83+
84+
/*
85+
ring buffer for IMU data,
86+
*/
87+
class ekf_imu_buffer
88+
{
89+
public:
90+
ekf_imu_buffer(uint8_t elsize);
91+
92+
// initialise buffer, returns false when allocation has failed
93+
bool init(uint32_t size);
94+
95+
/*
96+
Writes data to a Ring buffer and advances indices that
97+
define the location of the newest and oldest data
98+
*/
99+
void push_youngest_element(const void *element);
100+
101+
// return true if the buffer has been filled at least once
102+
bool is_filled(void) const {
103+
return _filled;
104+
}
105+
106+
// retrieve the oldest data from the ring buffer tail
107+
void get_oldest_element(void *element);
108+
109+
// writes the same data to all elements in the ring buffer
110+
void reset_history(const void *element);
111+
112+
// zeroes all data in the ring buffer
113+
void reset();
114+
115+
// retrieves data from the ring buffer at a specified index
116+
void *get(uint8_t index) const;
117+
118+
// returns the index for the ring buffer oldest data
119+
uint8_t get_oldest_index() const {
120+
return _oldest;
121+
}
122+
123+
// returns the index for the ring buffer youngest data
124+
uint8_t get_youngest_index() const {
125+
return _youngest;
126+
}
127+
128+
protected:
129+
const uint8_t elsize;
130+
void *buffer;
131+
uint8_t _size,_oldest,_youngest;
132+
bool _filled;
133+
134+
void *get_offset(uint8_t idx) const;
135+
};
136+
137+
/*
138+
template class for more convenient type handling
139+
*/
140+
template <typename element_type>
141+
class EKF_IMU_buffer_t : ekf_imu_buffer
142+
{
143+
public:
144+
EKF_IMU_buffer_t() :
145+
ekf_imu_buffer(sizeof(element_type))
146+
{}
147+
148+
bool init(uint8_t size) {
149+
return ekf_imu_buffer::init(size);
150+
}
151+
152+
/*
153+
Writes data to a Ring buffer and advances indices that
154+
define the location of the newest and oldest data
155+
*/
156+
void push_youngest_element(element_type element) {
157+
return ekf_imu_buffer::push_youngest_element(&element);
158+
}
159+
160+
// return true if the buffer has been filled at least once
161+
bool is_filled(void) const {
162+
return ekf_imu_buffer::is_filled();
163+
}
164+
165+
// retrieve the oldest data from the ring buffer tail
166+
element_type get_oldest_element() {
167+
element_type ret;
168+
ekf_imu_buffer::get_oldest_element(&ret);
169+
return ret;
170+
}
171+
172+
// writes the same data to all elements in the ring buffer
173+
void reset_history(element_type element) {
174+
ekf_imu_buffer::reset_history(&element);
175+
}
176+
177+
// zeroes all data in the ring buffer
178+
void reset() {
179+
ekf_imu_buffer::reset();
180+
}
181+
182+
// retrieves data from the ring buffer at a specified index
183+
element_type& operator[](uint32_t index) {
184+
element_type *ret = (element_type *)ekf_imu_buffer::get(index);
185+
return *ret;
186+
}
187+
188+
// returns the index for the ring buffer oldest data
189+
uint8_t get_oldest_index() {
190+
return ekf_imu_buffer::get_oldest_index();
191+
}
192+
193+
// returns the index for the ring buffer youngest data
194+
inline uint8_t get_youngest_index() {
195+
return ekf_imu_buffer::get_youngest_index();
196+
}
197+
};

0 commit comments

Comments
 (0)