Line data Source code
1 : #pragma once
2 :
3 : #include <atomic>
4 : #include <stdint.h>
5 : #include <AP_HAL/AP_HAL_Boards.h>
6 : #include <AP_HAL/AP_HAL_Macros.h>
7 : #include <AP_HAL/Semaphores.h>
8 :
9 : /*
10 : * Circular buffer of bytes.
11 : */
12 : class ByteBuffer {
13 : public:
14 : ByteBuffer(uint32_t size);
15 2 : ByteBuffer(uint8_t* _buf, uint32_t _size) :
16 2 : buf(_buf),
17 2 : size(_size),
18 2 : external_buf(true)
19 2 : {}
20 : ~ByteBuffer(void);
21 :
22 : // number of bytes available to be read
23 : uint32_t available(void) const;
24 :
25 : // Discards the buffer content, emptying it.
26 : void clear(void);
27 :
28 : // number of bytes space available to write
29 : uint32_t space(void) const;
30 :
31 : // true if available() is zero
32 : bool is_empty(void) const WARN_IF_UNUSED;
33 :
34 : // write bytes to ringbuffer. Returns number of bytes written
35 : uint32_t write(const uint8_t *data, uint32_t len);
36 :
37 : // read bytes from ringbuffer. Returns number of bytes read
38 : uint32_t read(uint8_t *data, uint32_t len);
39 :
40 : // read a byte from ring buffer. Returns true on success, false otherwise
41 : bool read_byte(uint8_t *data) WARN_IF_UNUSED;
42 :
43 : /*
44 : update bytes at the read pointer. Used to update an object without
45 : popping it
46 : */
47 : bool update(const uint8_t *data, uint32_t len);
48 :
49 : // return size of ringbuffer
50 192255345 : uint32_t get_size(void) const { return size; }
51 :
52 : // set size of ringbuffer, caller responsible for locking
53 : bool set_size(uint32_t size);
54 :
55 : // set size of ringbuffer, reducing down if size can't be achieved
56 : bool set_size_best(uint32_t size);
57 :
58 : // advance the read pointer (discarding bytes)
59 : bool advance(uint32_t n);
60 :
61 : // Returns the pointer and size to a contiguous read of the next available data
62 : const uint8_t *readptr(uint32_t &available_bytes);
63 :
64 : // peek one byte without advancing read pointer. Return byte
65 : // or -1 if none available
66 : int16_t peek(uint32_t ofs) const;
67 :
68 : /*
69 : read len bytes without advancing the read pointer
70 : */
71 : uint32_t peekbytes(uint8_t *data, uint32_t len);
72 :
73 : // Similar to peekbytes(), but will fill out IoVec struct with
74 : // both parts of the ring buffer if wraparound is happening, or
75 : // just one part. Returns the number of parts written to.
76 : struct IoVec {
77 : uint8_t *data;
78 : uint32_t len;
79 : };
80 : uint8_t peekiovec(IoVec vec[2], uint32_t len);
81 :
82 : // Reserve `len` bytes and fills out `vec` with both parts of the
83 : // ring buffer (if wraparound is happening), or just one contiguous
84 : // part. Returns the number of `vec` elements filled out. Can be used
85 : // with system calls such as `readv()`.
86 : //
87 : // After a call to 'reserve()', 'write()' should never be called
88 : // until 'commit()' is called!
89 : uint8_t reserve(IoVec vec[2], uint32_t len);
90 :
91 : /*
92 : * "Releases" the memory previously reserved by 'reserve()' to be read.
93 : * Committer must inform how many bytes were actually written in 'len'.
94 : */
95 : bool commit(uint32_t len);
96 :
97 : private:
98 : uint8_t *buf;
99 : uint32_t size;
100 :
101 : std::atomic<uint32_t> head{0}; // where to read data
102 : std::atomic<uint32_t> tail{0}; // where to write data
103 :
104 : bool external_buf;
105 : };
106 :
107 : /*
108 : ring buffer class for objects of fixed size
109 : !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
110 : */
111 : template <class T>
112 : class ObjectBuffer {
113 : public:
114 42200 : ObjectBuffer(uint32_t _size = 0) {
115 : // we set size to 1 more than requested as the byte buffer
116 : // gives one less byte than requested. We round up to a full
117 : // multiple of the object size so that we always get aligned
118 : // elements, which makes the readptr() method possible
119 42200 : buffer = NEW_NOTHROW ByteBuffer(((_size+1) * sizeof(T)));
120 42200 : external_buf = false;
121 42200 : }
122 :
123 : ObjectBuffer(ByteBuffer *_buffer) :
124 : buffer(_buffer),
125 : external_buf(true)
126 : {}
127 :
128 7567 : ~ObjectBuffer(void) {
129 7567 : if (!external_buf)
130 7567 : delete buffer;
131 7567 : }
132 :
133 : // return size of ringbuffer
134 10610263 : uint32_t get_size(void) const {
135 10610263 : if (buffer == nullptr) {
136 0 : return 0;
137 : }
138 10610263 : uint32_t size = buffer->get_size() / sizeof(T);
139 10610263 : return size>0?size-1:0;
140 : }
141 :
142 : // set size of ringbuffer, caller responsible for locking
143 191 : bool set_size(uint32_t size) { return buffer->set_size(((size+1) * sizeof(T))); }
144 :
145 : // read len objects without advancing the read pointer
146 281246 : uint32_t peek(T *data, uint32_t len) { return buffer->peekbytes((uint8_t*)data, len * sizeof(T)) / sizeof(T); }
147 :
148 : // Discards the buffer content, emptying it.
149 : // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
150 1769 : void clear(void)
151 : {
152 1769 : buffer->clear();
153 1769 : }
154 :
155 : // return number of objects available to be read from the front of the queue
156 : // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
157 536514815 : uint32_t available(void) const {
158 536514815 : return buffer->available() / sizeof(T);
159 : }
160 :
161 : // return number of objects that could be written to the back of the queue
162 : // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
163 255021019 : uint32_t space(void) const {
164 255021019 : return buffer->space() / sizeof(T);
165 : }
166 :
167 : // true is available() == 0
168 : // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
169 2390 : bool is_empty(void) const WARN_IF_UNUSED {
170 2390 : return buffer->is_empty();
171 : }
172 :
173 : // push one object onto the back of the queue
174 : // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
175 31630102 : bool push(const T &object) {
176 31630102 : if (buffer->space() < sizeof(T)) {
177 5634685 : return false;
178 : }
179 25995417 : return buffer->write((uint8_t*)&object, sizeof(T)) == sizeof(T);
180 : }
181 :
182 : // push N objects onto the back of the queue
183 : // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
184 : bool push(const T *object, uint32_t n) {
185 : if (buffer->space() < n*sizeof(T)) {
186 : return false;
187 : }
188 : return buffer->write((uint8_t*)object, n*sizeof(T)) == n*sizeof(T);
189 : }
190 :
191 : /*
192 : throw away an object from the front of the queue
193 : */
194 : // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
195 25297 : bool pop(void) {
196 25297 : return buffer->advance(sizeof(T));
197 : }
198 :
199 : /*
200 : pop earliest object off the front of the queue
201 : */
202 : // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
203 254343839 : bool pop(T &object) WARN_IF_UNUSED {
204 254343839 : if (buffer->available() < sizeof(T)) {
205 234002237 : return false;
206 : }
207 20341602 : return buffer->read((uint8_t*)&object, sizeof(T)) == sizeof(T);
208 : }
209 :
210 :
211 : /*
212 : * push_force() is semantically equivalent to:
213 : * if (!push(t)) { pop(); push(t); }
214 : */
215 : // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
216 113 : bool push_force(const T &object) {
217 113 : if (buffer->space() < sizeof(T)) {
218 86 : buffer->advance(sizeof(T));
219 : }
220 113 : return push(object);
221 : }
222 :
223 : /*
224 : * push_force() N objects
225 : */
226 : // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
227 : bool push_force(const T *object, uint32_t n) {
228 : uint32_t _space = buffer->space();
229 : if (_space < sizeof(T)*n) {
230 : buffer->advance(sizeof(T)*(n-_space));
231 : }
232 : return push(object, n);
233 : }
234 :
235 : /*
236 : peek copies an object out from the front of the queue without advancing the read pointer
237 : */
238 : // !!! Note ObjectBuffer_TS is a duplicate of this update, in both places !!!
239 1648500 : bool peek(T &object) WARN_IF_UNUSED {
240 1648500 : return buffer->peekbytes((uint8_t*)&object, sizeof(T)) == sizeof(T);
241 : }
242 :
243 : /*
244 : return a pointer to first contiguous array of available
245 : objects. Return nullptr if none available
246 : */
247 : // !!! Note ObjectBuffer_TS is a duplicate of this, update in both places !!!
248 : const T *readptr(uint32_t &n) {
249 : uint32_t avail_bytes = 0;
250 : #pragma GCC diagnostic push
251 : #pragma GCC diagnostic ignored "-Wcast-align"
252 : const T *ret = (const T *)buffer->readptr(avail_bytes);
253 : #pragma GCC diagnostic pop
254 : if (!ret || avail_bytes < sizeof(T)) {
255 : return nullptr;
256 : }
257 : n = avail_bytes / sizeof(T);
258 : return ret;
259 : }
260 :
261 : // advance the read pointer (discarding objects)
262 : // !!! Note ObjectBuffer_TS is a duplicate of this, update in both places !!!
263 281527 : bool advance(uint32_t n) {
264 281527 : return buffer->advance(n * sizeof(T));
265 : }
266 :
267 : /* update the object at the front of the queue (the one that would
268 : be fetched by pop()) */
269 : // !!! Note ObjectBuffer_TS is a duplicate of this, update in both places !!!
270 : bool update(const T &object) {
271 : return buffer->update((uint8_t*)&object, sizeof(T));
272 : }
273 :
274 : private:
275 : ByteBuffer *buffer = nullptr;
276 : bool external_buf = true;
277 : };
278 :
279 : /*
280 : Thread safe ring buffer class for objects of fixed size
281 : !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
282 : */
283 : template <class T>
284 : class ObjectBuffer_TS {
285 : public:
286 4392 : ObjectBuffer_TS(uint32_t _size = 0) {
287 : // we set size to 1 more than requested as the byte buffer
288 : // gives one less byte than requested. We round up to a full
289 : // multiple of the object size so that we always get aligned
290 : // elements, which makes the readptr() method possible
291 4392 : buffer = NEW_NOTHROW ByteBuffer(((_size+1) * sizeof(T)));
292 4392 : }
293 1106 : ~ObjectBuffer_TS(void) {
294 1106 : delete buffer;
295 1106 : }
296 :
297 : // return size of ringbuffer
298 : uint32_t get_size(void) {
299 : WITH_SEMAPHORE(sem);
300 : if (buffer == nullptr) {
301 : return 0;
302 : }
303 : uint32_t size = buffer->get_size() / sizeof(T);
304 : return size>0?size-1:0;
305 : }
306 :
307 : // set size of ringbuffer, caller responsible for locking
308 : bool set_size(uint32_t size) {
309 : WITH_SEMAPHORE(sem);
310 : return buffer->set_size(((size+1) * sizeof(T)));
311 : }
312 :
313 : // read len objects without advancing the read pointer
314 : uint32_t peek(T *data, uint32_t len) {
315 : WITH_SEMAPHORE(sem);
316 : return buffer->peekbytes((uint8_t*)data, len * sizeof(T)) / sizeof(T);
317 : }
318 :
319 :
320 : // Discards the buffer content, emptying it.
321 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
322 : void clear(void)
323 : {
324 : WITH_SEMAPHORE(sem);
325 : buffer->clear();
326 : }
327 :
328 : // return number of objects available to be read from the front of the queue
329 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
330 39460 : uint32_t available(void) {
331 39460 : WITH_SEMAPHORE(sem);
332 39460 : return buffer->available() / sizeof(T);
333 39460 : }
334 :
335 : // return number of objects that could be written to the back of the queue
336 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
337 6 : uint32_t space(void) {
338 6 : WITH_SEMAPHORE(sem);
339 6 : return buffer->space() / sizeof(T);
340 6 : }
341 :
342 : // true is available() == 0
343 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
344 474 : bool is_empty(void) WARN_IF_UNUSED {
345 474 : WITH_SEMAPHORE(sem);
346 474 : return buffer->is_empty();
347 474 : }
348 :
349 : // push one object onto the back of the queue
350 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
351 70811 : bool push(const T &object) {
352 70811 : WITH_SEMAPHORE(sem);
353 70811 : if (buffer->space() < sizeof(T)) {
354 3 : return false;
355 : }
356 70808 : return buffer->write((uint8_t*)&object, sizeof(T)) == sizeof(T);
357 70811 : }
358 :
359 : // push N objects onto the back of the queue
360 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
361 : bool push(const T *object, uint32_t n) {
362 : WITH_SEMAPHORE(sem);
363 : if (buffer->space() < n*sizeof(T)) {
364 : return false;
365 : }
366 : return buffer->write((uint8_t*)object, n*sizeof(T)) == n*sizeof(T);
367 : }
368 :
369 : /*
370 : throw away an object from the front of the queue
371 : */
372 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
373 18 : bool pop(void) {
374 18 : WITH_SEMAPHORE(sem);
375 18 : return buffer->advance(sizeof(T));
376 18 : }
377 :
378 : /*
379 : pop earliest object off the front of the queue
380 : */
381 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
382 243617842 : bool pop(T &object) WARN_IF_UNUSED {
383 243617842 : WITH_SEMAPHORE(sem);
384 243617842 : if (buffer->available() < sizeof(T)) {
385 243547062 : return false;
386 : }
387 70780 : return buffer->read((uint8_t*)&object, sizeof(T)) == sizeof(T);
388 243617842 : }
389 :
390 : /*
391 : * push_force() is semantically equivalent to:
392 : * if (!push(t)) { pop(); push(t); }
393 : */
394 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
395 19 : bool push_force(const T &object) {
396 19 : WITH_SEMAPHORE(sem);
397 19 : if (buffer->space() < sizeof(T)) {
398 0 : buffer->advance(sizeof(T));
399 : }
400 19 : return push(object);
401 19 : }
402 :
403 : /*
404 : * push_force() N objects
405 : */
406 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
407 : bool push_force(const T *object, uint32_t n) {
408 : WITH_SEMAPHORE(sem);
409 : uint32_t _space = buffer->space();
410 : if (_space < sizeof(T)*n) {
411 : buffer->advance(sizeof(T)*(n-_space));
412 : }
413 : return push(object, n);
414 : }
415 :
416 : /*
417 : peek copies an object out from the front of the queue without advancing the read pointer
418 : */
419 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
420 73143 : bool peek(T &object) WARN_IF_UNUSED {
421 73143 : WITH_SEMAPHORE(sem);
422 73143 : return buffer->peekbytes((uint8_t*)&object, sizeof(T)) == sizeof(T);
423 73143 : }
424 :
425 : /*
426 : return a pointer to first contiguous array of available
427 : objects. Return nullptr if none available
428 : */
429 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
430 : const T *readptr(uint32_t &n) {
431 : WITH_SEMAPHORE(sem);
432 : uint32_t avail_bytes = 0;
433 : #pragma GCC diagnostic push
434 : #pragma GCC diagnostic ignored "-Wcast-align"
435 : const T *ret = (const T *)buffer->readptr(avail_bytes);
436 : #pragma GCC diagnostic pop
437 : if (!ret || avail_bytes < sizeof(T)) {
438 : return nullptr;
439 : }
440 : n = avail_bytes / sizeof(T);
441 : return ret;
442 : }
443 :
444 : // advance the read pointer (discarding objects)
445 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
446 : bool advance(uint32_t n) {
447 : WITH_SEMAPHORE(sem);
448 : return buffer->advance(n * sizeof(T));
449 : }
450 :
451 : /* update the object at the front of the queue (the one that would
452 : be fetched by pop()) */
453 : // !!! Note this is a duplicate of ObjectBuffer with semaphore, update in both places !!!
454 : bool update(const T &object) {
455 : WITH_SEMAPHORE(sem);
456 : return buffer->update((uint8_t*)&object, sizeof(T));
457 : }
458 :
459 : private:
460 : ByteBuffer *buffer = nullptr;
461 : HAL_Semaphore sem;
462 : };
463 :
464 : /*
465 : ring buffer class for objects of fixed size with pointer
466 : access. Note that this is not thread safe, buf offers efficient
467 : array-like access
468 : */
469 : template <class T>
470 : class ObjectArray {
471 : public:
472 6228 : ObjectArray(uint16_t size_) {
473 6228 : _size = size_;
474 6228 : _head = _count = 0;
475 17218 : _buffer = NEW_NOTHROW T[_size];
476 6228 : }
477 963 : ~ObjectArray(void) {
478 963 : delete[] _buffer;
479 963 : }
480 :
481 : // return total number of objects
482 : uint16_t size(void) const {
483 : return _size;
484 : }
485 :
486 : // return number of objects available to be read
487 37696100 : uint16_t available(void) const {
488 37696100 : return _count;
489 : }
490 :
491 : // return number of objects that could be written
492 1227925 : uint16_t space(void) const {
493 1227925 : return _size - _count;
494 : }
495 :
496 : // true is available() == 0
497 42728266 : bool is_empty(void) const WARN_IF_UNUSED {
498 42728266 : return _count == 0;
499 : }
500 :
501 : // push one object
502 623070 : bool push(const T &object) {
503 623070 : if (space() == 0) {
504 0 : return false;
505 : }
506 623070 : _buffer[(_head+_count)%_size] = object;
507 623070 : _count++;
508 623070 : return true;
509 : }
510 :
511 : /*
512 : throw away an object
513 : */
514 584760 : bool pop(void) WARN_IF_UNUSED {
515 584760 : if (is_empty()) {
516 0 : return false;
517 : }
518 584760 : _head = (_head+1) % _size;
519 584760 : _count--;
520 584760 : return true;
521 : }
522 :
523 : // Discards the buffer content, emptying it.
524 0 : void clear(void)
525 : {
526 0 : _head = _count = 0;
527 0 : }
528 :
529 : /*
530 : pop earliest object off the queue
531 : */
532 0 : bool pop(T &object) WARN_IF_UNUSED {
533 0 : if (is_empty()) {
534 0 : return false;
535 : }
536 0 : object = _buffer[_head];
537 0 : return pop();
538 : }
539 :
540 :
541 : /*
542 : * push_force() is semantically equivalent to:
543 : * if (!push(t)) { pop(); push(t); }
544 : */
545 604855 : bool push_force(const T &object) {
546 604855 : if (space() == 0) {
547 475765 : UNUSED_RESULT(pop());
548 : }
549 604855 : return push(object);
550 : }
551 :
552 : /*
553 : remove the Nth element from the array. First element is zero
554 : */
555 123046 : bool remove(uint16_t n) {
556 123046 : if (n >= _count) {
557 0 : return false;
558 : }
559 123046 : if (n == _count-1) {
560 : // remove last element
561 31653 : _count--;
562 31653 : return true;
563 : }
564 91393 : if (n == 0) {
565 : // remove first element
566 90780 : return pop();
567 : }
568 : // take advantage of the [] operator for simple shift of the array elements
569 1300 : for (uint16_t i=n; i<_count-1; i++) {
570 687 : *(*this)[i] = *(*this)[i+1];
571 : }
572 613 : _count--;
573 613 : return true;
574 : }
575 :
576 : // allow array indexing, based on current head. Returns a pointer
577 : // to the object or nullptr
578 22565012 : T * operator[](uint16_t i) {
579 22565012 : if (i >= _count) {
580 0 : return nullptr;
581 : }
582 22565012 : return &_buffer[(_head+i)%_size];
583 : }
584 :
585 : private:
586 : T *_buffer;
587 : uint16_t _size; // total buffer size
588 : uint16_t _count; // number in buffer now
589 : uint16_t _head; // first element
590 : };
591 :
592 : typedef ObjectBuffer<float> FloatBuffer;
593 : typedef ObjectBuffer_TS<float> FloatBuffer_TS;
594 : typedef ObjectArray<float> FloatArray;
|
