PX4 Firmware
PX4 Autopilot Software http://px4.io
RingBuffer.h
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (C) 2015 PX4 Development Team. All rights reserved.
4  *
5  * Redistribution and use in source and binary forms, with or without
6  * modification, are permitted provided that the following conditions
7  * are met:
8  *
9  * 1. Redistributions of source code must retain the above copyright
10  * notice, this list of conditions and the following disclaimer.
11  * 2. Redistributions in binary form must reproduce the above copyright
12  * notice, this list of conditions and the following disclaimer in
13  * the documentation and/or other materials provided with the
14  * distribution.
15  * 3. Neither the name PX4 nor the names of its contributors may be
16  * used to endorse or promote products derived from this software
17  * without specific prior written permission.
18  *
19  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
20  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
21  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
22  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
23  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
24  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
25  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
26  * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
27  * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
28  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
29  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
30  * POSSIBILITY OF SUCH DAMAGE.
31  *
32  ****************************************************************************/
33 
34 /**
35  * @file RingBuffer.h
36  * @author Roman Bapst <bapstroman@gmail.com>
37  * Template RingBuffer.
38  */
39 
40 #include <inttypes.h>
41 #include <cstdio>
42 #include <cstring>
43 
44 template <typename data_type>
46 {
47 public:
49  {
50  if (allocate(1)) {
51  // initialize with one empty sample
52  data_type d = {};
53  push(d);
54  }
55  }
56  ~RingBuffer() { delete[] _buffer; }
57 
58  // no copy, assignment, move, move assignment
59  RingBuffer(const RingBuffer &) = delete;
60  RingBuffer &operator=(const RingBuffer &) = delete;
61  RingBuffer(RingBuffer &&) = delete;
62  RingBuffer &operator=(RingBuffer &&) = delete;
63 
64  bool allocate(uint8_t size)
65  {
66  if (_buffer != nullptr) {
67  delete[] _buffer;
68  }
69 
70  _buffer = new data_type[size];
71 
72  if (_buffer == nullptr) {
73  return false;
74  }
75 
76  _size = size;
77 
78  _head = 0;
79  _tail = 0;
80 
81  // set the time elements to zero so that bad data is not retrieved from the buffers
82  for (uint8_t index = 0; index < _size; index++) {
83  _buffer[index] = {};
84  }
85 
86  _first_write = true;
87 
88  return true;
89  }
90 
91  void unallocate()
92  {
93  delete[] _buffer;
94  _buffer = nullptr;
95  }
96 
97  void push(const data_type &sample)
98  {
99  uint8_t head_new = _head;
100 
101  if (!_first_write) {
102  head_new = (_head + 1) % _size;
103  }
104 
105  _buffer[head_new] = sample;
106  _head = head_new;
107 
108  // move tail if we overwrite it
109  if (_head == _tail && !_first_write) {
110  _tail = (_tail + 1) % _size;
111 
112  } else {
113  _first_write = false;
114  }
115  }
116 
117  uint8_t get_length() const { return _size; }
118 
119  data_type &operator[](const uint8_t index) { return _buffer[index]; }
120 
121  const data_type &get_newest() { return _buffer[_head]; }
122  const data_type &get_oldest() { return _buffer[_tail]; }
123 
124  uint8_t get_oldest_index() const { return _tail; }
125 
126  bool pop_first_older_than(const uint64_t &timestamp, data_type *sample)
127  {
128  // start looking from newest observation data
129  for (uint8_t i = 0; i < _size; i++) {
130  int index = (_head - i);
131  index = index < 0 ? _size + index : index;
132 
133  if (timestamp >= _buffer[index].time_us && timestamp - _buffer[index].time_us < (uint64_t)1e5) {
134 
135  *sample = _buffer[index];
136 
137  // Now we can set the tail to the item which comes after the one we removed
138  // since we don't want to have any older data in the buffer
139  if (index == _head) {
140  _tail = _head;
141  _first_write = true;
142 
143  } else {
144  _tail = (index + 1) % _size;
145  }
146 
147  _buffer[index].time_us = 0;
148 
149  return true;
150  }
151 
152  if (index == _tail) {
153  // we have reached the tail and haven't got a match
154  return false;
155  }
156  }
157 
158  return false;
159  }
160 
161  int get_total_size() { return sizeof(*this) + sizeof(data_type) * _size; }
162 
163 private:
164  data_type *_buffer{nullptr};
165 
166  uint8_t _head{0};
167  uint8_t _tail{0};
168  uint8_t _size{0};
169 
170  bool _first_write{true};
171 };
bool pop_first_older_than(const uint64_t &timestamp, data_type *sample)
Definition: RingBuffer.h:126
const data_type & get_newest()
Definition: RingBuffer.h:121
uint8_t _tail
Definition: RingBuffer.h:167
uint8_t _head
Definition: RingBuffer.h:166
void unallocate()
Definition: RingBuffer.h:91
uint8_t _size
Definition: RingBuffer.h:168
bool allocate(uint8_t size)
Definition: RingBuffer.h:64
data_type * _buffer
Definition: RingBuffer.h:164
const data_type & get_oldest()
Definition: RingBuffer.h:122
bool _first_write
Definition: RingBuffer.h:170
uint8_t get_oldest_index() const
Definition: RingBuffer.h:124
int get_total_size()
Definition: RingBuffer.h:161
data_type & operator[](const uint8_t index)
Definition: RingBuffer.h:119
RingBuffer & operator=(const RingBuffer &)=delete
uint8_t get_length() const
Definition: RingBuffer.h:117
void push(const data_type &sample)
Definition: RingBuffer.h:97