PX4 Firmware
PX4 Autopilot Software http://px4.io
test_EKF_ringbuffer.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2019 ECL 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 #include <gtest/gtest.h>
35 #include <math.h>
36 #include "EKF/ekf.h"
37 
38 struct sample {
39  uint64_t time_us;
40  float data[3];
41 };
42 
43 
44 class EkfRingBufferTest : public ::testing::Test {
45  public:
46 
47  sample _x, _y, _z;
49 
50  void SetUp() override
51  {
52  _buffer = new RingBuffer<sample>();
53  _x.time_us = 1000000;
54  _x.data[0] = _x.data[1] = _x.data[2] = 1.5f;
55 
56  _y.time_us = 2000000;
57  _y.data[0] = _y.data[1] = _y.data[2] = 3.0f;
58 
59  _z.time_us = 3000000;
60  _z.data[0] = _z.data[1] = _z.data[2] = 4.0f;
61  }
62 
63  void TearDown() override
64  {
65  _buffer->unallocate();
66  delete _buffer;
67  }
68 };
69 
70 TEST_F(EkfRingBufferTest, goodInitialisation)
71 {
72  // WHEN: buffer was allocated
73  // THEN: allocation should have succeed
74  ASSERT_EQ(true, _buffer->allocate(3));
75 
76 }
77 
78 TEST_F(EkfRingBufferTest, badInitialisation)
79 {
80  // WHEN: buffer allocation input is bad
81  // THEN: allocation should fail
82 
83  // TODO: Change buffer implementation to pass this test
84  // ASSERT_EQ(false, _buffer->allocate(-1));
85  // ASSERT_EQ(false, _buffer->allocate(0));
86 }
87 
88 TEST_F(EkfRingBufferTest, orderOfSamples)
89 {
90  ASSERT_EQ(true, _buffer->allocate(3));
91  // GIVEN: allocated buffer
92  // WHEN: adding multiple samples
93  // THEN: they should be added in order
94  _buffer->push(_x);
95 
96  EXPECT_EQ(_x.time_us, _buffer->get_newest().time_us);
97  EXPECT_EQ(_x.time_us, _buffer->get_oldest().time_us);
98 
99  _buffer->push(_z);
100  _buffer->push(_y);
101 
102  EXPECT_EQ(_x.time_us, _buffer->get_oldest().time_us);
103  EXPECT_EQ(_y.time_us, _buffer->get_newest().time_us);
104 }
105 
107 {
108  ASSERT_EQ(true, _buffer->allocate(3));
109  _buffer->push(_x);
110  _buffer->push(_y);
111  _buffer->push(_z);
112 
113  // GIVEN: allocated and filled buffer
114 
115  sample pop = {};
116  // WHEN: we want to retrieve a sample that is older than any in the buffer
117  // THEN: we should not get any sample
118  EXPECT_EQ(false, _buffer->pop_first_older_than(0, &pop));
119 
120  // WHEN: when calling "pop_first_older_than"
121  // THEN: we should get the first sample from the head that is older
122  EXPECT_EQ(true, _buffer->pop_first_older_than(_x.time_us+1, &pop));
123  EXPECT_EQ(_x.time_us, pop.time_us);
124  EXPECT_EQ(true, _buffer->pop_first_older_than(_y.time_us+10, &pop));
125  EXPECT_EQ(_y.time_us, pop.time_us);
126  EXPECT_EQ(true, _buffer->pop_first_older_than(_z.time_us+100, &pop));
127  EXPECT_EQ(_z.time_us, pop.time_us);
128  // TODO: When changing the order of popping sample it does not behave as expected, fix this
129 }
130 
131 TEST_F(EkfRingBufferTest, askingForTooNewSample)
132 {
133  ASSERT_EQ(true, _buffer->allocate(3));
134  _buffer->push(_x);
135  _buffer->push(_y);
136  _buffer->push(_z);
137 
138  sample pop = {};
139  // WHEN: all buffered samples are older by 0.1s than your query timestamp
140  // THEN: should get no sample returned
141  EXPECT_EQ(true, _buffer->pop_first_older_than(_z.time_us+99000, &pop));
142  EXPECT_EQ(false, _buffer->pop_first_older_than(_y.time_us+100000, &pop));
143 }
144 
145 TEST_F(EkfRingBufferTest, reallocateBuffer)
146 {
147  ASSERT_EQ(true, _buffer->allocate(5));
148  _buffer->push(_x);
149  _buffer->push(_y);
150  _buffer->push(_y);
151  _buffer->push(_y);
152  _buffer->push(_z);
153 
154  // GIVEN: allocated and filled buffer
155  // WHEN: do another allocate call
156  _buffer->allocate(3);
157  // THEN: its length should update
158  EXPECT_EQ(3,_buffer->get_length());
159 
160 }
void unallocate()
Definition: RingBuffer.h:91
uint64_t time_us
RingBuffer< sample > * _buffer
TEST_F(EkfRingBufferTest, goodInitialisation)
Class for core functions for ekf attitude and position estimator.
void TearDown() override