PX4 Firmware
PX4 Autopilot Software http://px4.io
test_data_validator.cpp
Go to the documentation of this file.
1 /****************************************************************************
2  *
3  * Copyright (c) 2019 Todd Stellanova. 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 of the copyright holder nor the names of its
16  * contributors may be used to endorse or promote products derived
17  * from this software 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  * @file test_data_validator.cpp
35  * Testing the DataValidator class
36  *
37  * @author Todd Stellanova
38  */
39 
40 #include <stdint.h>
41 #include <cassert>
42 #include <cstdlib>
43 #include <stdio.h>
44 #include <math.h>
47 
48 
49 void test_init()
50 {
51  printf("\n--- test_init ---\n");
52 
53  uint64_t fake_timestamp = 666;
54  const uint32_t timeout_usec = 2000;//from original private value
55 
56  DataValidator *validator = new DataValidator;
57  // initially there should be no siblings
58  assert(nullptr == validator->sibling());
59  // initially we should have zero confidence
60  assert(0.0f == validator->confidence(fake_timestamp));
61  // initially the error count should be zero
62  assert(0 == validator->error_count());
63  // initially unused
64  assert(!validator->used());
65  // initially no priority
66  assert(0 == validator->priority());
67  validator->set_timeout(timeout_usec);
68  assert(validator->get_timeout() == timeout_usec);
69 
70 
71  DataValidator *sibling_validator = new DataValidator;
72  validator->setSibling(sibling_validator);
73  assert(sibling_validator == validator->sibling());
74 
75  //verify that with no data, confidence is zero and error mask is set
76  assert(0.0f == validator->confidence(fake_timestamp + 1));
77  uint32_t state = validator->state();
79 
80  //verify that calling print doesn't crash tests
81  validator->print();
82 
83  float *vibe_offset = validator->vibration_offset();
84  assert(0.0f == vibe_offset[0]);
85 
86  delete validator; //force delete
87 
88 }
89 
90 void test_put()
91 {
92  printf("\n--- test_put ---\n");
93 
94  uint64_t timestamp = 500;
95  const uint32_t timeout_usec = 2000;//derived from class-private value
96  float val = 3.14159f;
97  //derived from class-private value: this is min change needed to avoid stale detection
98  const float sufficient_incr_value = (1.1f * 1E-6f);
99 
100  DataValidator *validator = new DataValidator;
101  fill_validator_with_samples(validator, sufficient_incr_value, &val, &timestamp);
102 
103  assert(validator->used());
104  //verify that the last value we inserted is the current validator value
105  float last_val = val - sufficient_incr_value;
106  assert(validator->value()[0] == last_val);
107 
108  // we've just provided a bunch of valid data: should be fully confident
109  float conf = validator->confidence(timestamp);
110 
111  if (1.0f != conf) {
112  printf("conf: %f\n", (double)conf);
113  dump_validator_state(validator);
114  }
115 
116  assert(1.0f == conf);
117  // should be no errors
118  assert(0 == validator->state());
119 
120  //now check confidence much beyond the timeout window-- should timeout
121  conf = validator->confidence(timestamp + (1.1 * timeout_usec));
122 
123  if (0.0f != conf) {
124  printf("conf: %f\n", (double)conf);
125  dump_validator_state(validator);
126  }
127 
128  assert(0.0f == conf);
130 
131  delete validator; //force delete
132 
133 }
134 
135 /**
136  * Verify that the DataValidator detects sensor data that does not vary sufficiently
137  */
139 {
140  printf("\n--- test_stale_detector ---\n");
141 
142  uint64_t timestamp = 500;
143  float val = 3.14159f;
144  //derived from class-private value, this is insufficient to avoid stale detection:
145  const float insufficient_incr_value = (0.99 * 1E-6f);
146 
147  DataValidator *validator = new DataValidator;
148  fill_validator_with_samples(validator, insufficient_incr_value, &val, &timestamp);
149 
150  // data is stale: should have no confidence
151  assert(0.0f == validator->confidence(timestamp));
152 
153  // should be a stale error
154  uint32_t state = validator->state();
155 
157  dump_validator_state(validator);
158  }
159 
161 
162  delete validator; //force delete
163 
164 }
165 
166 /**
167  * Verify the RMS error calculated by the DataValidator for a series of samples
168  */
170 {
171  printf("\n--- test_rms_calculation ---\n");
172  const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT
173  const float mean_value = 3.14159f;
174  const uint32_t sample_count = 1000;
175  float expected_rms_err = 0.0f;
176  uint64_t timestamp = 500;
177 
178  DataValidator *validator = new DataValidator;
179  validator->set_equal_value_threshold(equal_value_count);
180 
181  insert_values_around_mean(validator, mean_value, sample_count, &expected_rms_err, &timestamp);
182  float *rms = validator->rms();
183  assert(nullptr != rms);
184  float calc_rms_err = rms[0];
185  float diff = fabsf(calc_rms_err - expected_rms_err);
186  float diff_frac = (diff / expected_rms_err);
187  printf("rms: %f expect: %f diff: %f frac: %f\n", (double)calc_rms_err, (double)expected_rms_err,
188  (double)diff, (double)diff_frac);
189  assert(diff_frac < 0.03f);
190 
191  float *vibe_offset = validator->vibration_offset();
192  float vibe_diff = fabsf(0.01005f - vibe_offset[0]); //TODO calculate this vibration value
193  printf("vibe: %f", (double)vibe_offset[0]);
194  assert(vibe_diff < 1E-3f);
195 
196  delete validator; //force delete
197 
198 }
199 
200 /**
201  * Verify error tracking performed by DataValidator::put
202  */
204 {
205  printf("\n--- test_error_tracking ---\n");
206  uint64_t timestamp = 500;
207  uint64_t timestamp_incr = 5;
208  const uint32_t timeout_usec = 2000;//from original private value
209  float val = 3.14159f;
210  uint64_t error_count = 0;
211  int expected_error_density = 0;
212  int priority = 50;
213  //from private value: this is min change needed to avoid stale detection
214  const float sufficient_incr_value = (1.1f * 1E-6f);
215  //default is private VALUE_EQUAL_COUNT_DEFAULT
216  const int equal_value_count = 50000;
217  //should be less than equal_value_count: ensure this is less than NORETURN_ERRCOUNT
218  const int total_iterations = 1000;
219 
220  DataValidator *validator = new DataValidator;
221  validator->set_timeout(timeout_usec);
222  validator->set_equal_value_threshold(equal_value_count);
223 
224  //put a bunch of values that are all different
225  for (int i = 0; i < total_iterations; i++, val += sufficient_incr_value) {
226  timestamp += timestamp_incr;
227 
228  //up to a 50% random error rate appears to pass the error density filter
229  if ((((float)rand() / (float)RAND_MAX)) < 0.500f) {
230  error_count += 1;
231  expected_error_density += 1;
232 
233  } else if (expected_error_density > 0) {
234  expected_error_density -= 1;
235  }
236 
237  validator->put(timestamp, val, error_count, priority);
238  }
239 
240  assert(validator->used());
241  //at this point, error_count should be less than NORETURN_ERRCOUNT
242  assert(validator->error_count() == error_count);
243 
244  // we've just provided a bunch of valid data with some errors:
245  // confidence should be reduced by the number of errors
246  float conf = validator->confidence(timestamp);
247  printf("error_count: %u validator confidence: %f\n", (uint32_t)error_count, (double)conf);
248  assert(1.0f != conf); //we should not be fully confident
249  assert(0.0f != conf); //neither should we be completely unconfident
250  // should be no errors, even if confidence is reduced, since we didn't exceed NORETURN_ERRCOUNT
251  assert(0 == validator->state());
252 
253  // the error density will reduce the confidence by 1 - (error_density / ERROR_DENSITY_WINDOW)
254  // ERROR_DENSITY_WINDOW is currently private, but == 100.0f
255  float reduced_conf = 1.0f - ((float)expected_error_density / 100.0f);
256  double diff = fabs(reduced_conf - conf);
257 
258  if (reduced_conf != conf) {
259  printf("conf: %f reduced_conf: %f diff: %f\n",
260  (double)conf, (double)reduced_conf, diff);
261  dump_validator_state(validator);
262  }
263 
264  assert(diff < 1E-6f);
265 
266  //Now, insert a series of errors and ensure we trip the error detector
267  for (int i = 0; i < 250; i++, val += sufficient_incr_value) {
268  timestamp += timestamp_incr;
269  //100% error rate
270  error_count += 1;
271  expected_error_density += 1;
272  validator->put(timestamp, val, error_count, priority);
273  }
274 
275  conf = validator->confidence(timestamp);
276  assert(0.0f == conf); // should we be completely unconfident
277  // we should have triggered the high error density detector
279 
280 
281  validator->reset_state();
282 
283  //Now insert so many errors that we exceed private NORETURN_ERRCOUNT
284  for (int i = 0; i < 10000; i++, val += sufficient_incr_value) {
285  timestamp += timestamp_incr;
286  //100% error rate
287  error_count += 1;
288  expected_error_density += 1;
289  validator->put(timestamp, val, error_count, priority);
290  }
291 
292  conf = validator->confidence(timestamp);
293  assert(0.0f == conf); // should we be completely unconfident
294  // we should have triggered the high error count detector
296 
297  delete validator; //force delete
298 
299 }
300 
301 int main(int argc, char *argv[])
302 {
303  (void)argc; // unused
304  (void)argv; // unused
305 
306  srand(666);
307  test_init();
308  test_put();
312  //TODO verify vibration calculation
313 
314  return 0; //passed
315 }
int priority() const
Get the priority of this validator.
uint32_t get_timeout() const
Get the timeout value.
bool used() const
Get the used status of this validator.
void test_put()
static enum @74 state
void reset_state()
Reset the error state of this validator.
static constexpr uint32_t ERROR_FLAG_TIMEOUT
void dump_validator_state(DataValidator *validator)
Print out the state of a DataValidator.
void insert_values_around_mean(DataValidator *validator, const float mean, uint32_t count, float *rms_err, uint64_t *timestamp_io)
Insert a series of samples around a mean value.
const int equal_value_count
static constexpr uint32_t ERROR_FLAG_STALE_DATA
void test_stale_detector()
Verify that the DataValidator detects sensor data that does not vary sufficiently.
void test_init()
static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT
void print()
Print the validator value.
void set_timeout(uint32_t timeout_interval_us)
Set the timeout value.
void setSibling(DataValidator *new_sibling)
Set the sibling to the next node in the group.
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
uint32_t state() const
Get the error state of this validator.
float * rms()
Get the RMS values of this validator.
static constexpr uint32_t ERROR_FLAG_NO_DATA
float * value()
Get the values of this validator.
DataValidator * sibling()
Get the next sibling in the group.
static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY
void set_equal_value_threshold(uint32_t threshold)
Set the equal count threshold.
void test_rms_calculation()
Verify the RMS error calculated by the DataValidator for a series of samples.
float * vibration_offset()
Get the vibration offset.
void test_error_tracking()
Verify error tracking performed by DataValidator::put.
void put(uint64_t timestamp, float val, uint64_t error_count, int priority)
Put an item into the validator.
void fill_validator_with_samples(DataValidator *validator, const float incr_value, float *value_io, uint64_t *timestamp_io)
Insert a time series of samples into the validator.
A data validation class to identify anomalies in data streams.
float confidence(uint64_t timestamp)
Get the confidence of this validator.
int main(int argc, char *argv[])
uint64_t error_count() const
Get the error count of this validator.