PX4 Firmware
PX4 Autopilot Software http://px4.io
test_data_validator_group.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_group.cpp
35  * Testing the DataValidatorGroup 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>
48 
49 
50 const uint32_t base_timeout_usec = 2000;//from original private value
51 const int equal_value_count = 100; //default is private VALUE_EQUAL_COUNT_DEFAULT
52 const uint64_t base_timestamp = 666;
53 const unsigned base_num_siblings = 4;
54 
55 
56 /**
57  * Initialize a DataValidatorGroup with some common settings;
58  * @param sibling_count (out) the number of siblings initialized
59  */
60 DataValidatorGroup *setup_base_group(unsigned *sibling_count)
61 {
62  unsigned num_siblings = base_num_siblings;
63 
64  DataValidatorGroup *group = new DataValidatorGroup(num_siblings);
65  assert(nullptr != group);
66  //verify that calling print doesn't crash the tests
67  group->print();
68  printf("\n");
69 
70  //should be no failovers yet
71  assert(0 == group->failover_count());
73  assert(-1 == group->failover_index());
74 
75  //no vibration yet
76  float vibe_off = group->get_vibration_offset(base_timestamp, 0);
77  printf("vibe_off: %f \n", (double)vibe_off);
78  assert(-1.0f == group->get_vibration_offset(base_timestamp, 0));
79 
80  float vibe_fact = group->get_vibration_factor(base_timestamp);
81  printf("vibe_fact: %f \n", (double)vibe_fact);
82  assert(0.0f == vibe_fact);
83 
84  //this sets the timeout on all current members of the group, as well as members added later
86  //the following sets the threshold on all CURRENT members of the group, but not any added later
87  //TODO This is likely a bug in DataValidatorGroup
89 
90  //return values
91  *sibling_count = num_siblings;
92 
93  return group;
94 
95 }
96 
97 /**
98  * Fill one DataValidator with samples, by index.
99  *
100  * @param group
101  * @param val1_idx Index of the validator to fill with samples
102  * @param num_samples
103  */
104 void fill_one_with_valid_data(DataValidatorGroup *group, int val1_idx, uint32_t num_samples)
105 {
106  uint64_t timestamp = base_timestamp;
107  uint64_t error_count = 0;
108  float last_best_val = 0.0f;
109 
110  for (uint32_t i = 0; i < num_samples; i++) {
111  float val = ((float) rand() / (float) RAND_MAX);
112  float data[DataValidator::dimensions] = {val};
113  group->put(val1_idx, timestamp, data, error_count, 100);
114  last_best_val = val;
115  }
116 
117  int best_idx = 0;
118  float *best_data = group->get_best(timestamp, &best_idx);
119  assert(last_best_val == best_data[0]);
120  assert(best_idx == val1_idx);
121 }
122 
123 
124 
125 /**
126  * Fill two validators in the group with samples, by index.
127  * Both validators will be filled with the same data, but
128  * the priority of the first validator will be higher than the second.
129  *
130  * @param group
131  * @param val1_idx index of the first validator to fill
132  * @param val2_idx index of the second validator to fill
133  * @param num_samples
134  */
135 void fill_two_with_valid_data(DataValidatorGroup *group, int val1_idx, int val2_idx, uint32_t num_samples)
136 {
137  uint64_t timestamp = base_timestamp;
138  uint64_t error_count = 0;
139  float last_best_val = 0.0f;
140 
141  for (uint32_t i = 0; i < num_samples; i++) {
142  float val = ((float) rand() / (float) RAND_MAX);
143  float data[DataValidator::dimensions] = {val};
144  //two sensors with identical values, but different priorities
145  group->put(val1_idx, timestamp, data, error_count, 100);
146  group->put(val2_idx, timestamp, data, error_count, 10);
147  last_best_val = val;
148  }
149 
150  int best_idx = 0;
151  float *best_data = group->get_best(timestamp, &best_idx);
152  assert(last_best_val == best_data[0]);
153  assert(best_idx == val1_idx);
154 
155 }
156 
157 /**
158  * Dynamically add a validator to the group after construction
159  * @param group
160  * @return
161  */
163 {
164  DataValidator *validator = group->add_new_validator();
165  //verify the previously set timeout applies to the new group member
166  assert(validator->get_timeout() == base_timeout_usec);
167  //for testing purposes, ensure this newly added member is consistent with the rest of the group
168  //TODO this is likely a bug in DataValidatorGroup
170 
171  return validator;
172 }
173 
174 /**
175  * Create a DataValidatorGroup and tack on two additional DataValidators
176  *
177  * @param validator1_handle (out) first DataValidator added to the group
178  * @param validator2_handle (out) second DataValidator added to the group
179  * @param sibling_count (in/out) in: number of initial siblings to create, out: total
180  * @return
181  */
183  DataValidator **validator1_handle,
184  DataValidator **validator2_handle,
185  unsigned *sibling_count)
186 {
187  DataValidatorGroup *group = setup_base_group(sibling_count);
188 
189  //now we add validators
190  *validator1_handle = add_validator_to_group(group);
191  *validator2_handle = add_validator_to_group(group);
192  *sibling_count += 2;
193 
194  return group;
195 }
196 
197 
198 void test_init()
199 {
200  unsigned num_siblings = 0;
201 
202  DataValidatorGroup *group = setup_base_group(&num_siblings);
203 
204  //should not yet be any best value
205  int best_index = -1;
206  assert(nullptr == group->get_best(base_timestamp, &best_index));
207 
208  delete group; //force cleanup
209 }
210 
211 
212 /**
213  * Happy path test of put method -- ensure the "best" sensor selected is the one with highest priority
214  */
215 void test_put()
216 {
217  unsigned num_siblings = 0;
218  DataValidator *validator1 = nullptr;
219  DataValidator *validator2 = nullptr;
220 
221  uint64_t timestamp = base_timestamp;
222 
223  DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings);
224  printf("num_siblings: %d \n", num_siblings);
225  unsigned val1_idx = num_siblings - 2;
226  unsigned val2_idx = num_siblings - 1;
227 
228  fill_two_with_valid_data(group, val1_idx, val2_idx, 500);
229  int best_idx = -1;
230  float *best_data = group->get_best(timestamp, &best_idx);
231  assert(nullptr != best_data);
232  float best_val = best_data[0];
233 
234  float *cur_val1 = validator1->value();
235  assert(nullptr != cur_val1);
236  //printf("cur_val1 %p \n", cur_val1);
237  assert(best_val == cur_val1[0]);
238 
239  float *cur_val2 = validator2->value();
240  assert(nullptr != cur_val2);
241  //printf("cur_val12 %p \n", cur_val2);
242  assert(best_val == cur_val2[0]);
243 
244  delete group; //force cleanup
245 }
246 
247 
248 /**
249  * Verify that the DataValidatorGroup will select the sensor with the latest higher priority as "best".
250  */
252 {
253  unsigned num_siblings = 0;
254  DataValidator *validator1 = nullptr;
255  DataValidator *validator2 = nullptr;
256 
257  uint64_t timestamp = base_timestamp;
258 
259  DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings);
260  //printf("num_siblings: %d \n",num_siblings);
261  int val1_idx = (int)num_siblings - 2;
262  int val2_idx = (int)num_siblings - 1;
263  uint64_t error_count = 0;
264 
265  fill_two_with_valid_data(group, val1_idx, val2_idx, 100);
266 
267  int best_idx = -1;
268  float *best_data = nullptr;
269  //now, switch the priorities, which switches "best" but doesn't trigger a failover
270  float new_best_val = 3.14159f;
271  float data[DataValidator::dimensions] = {new_best_val};
272  //a single sample insertion should be sufficient to trigger a priority switch
273  group->put(val1_idx, timestamp, data, error_count, 1);
274  group->put(val2_idx, timestamp, data, error_count, 100);
275  best_data = group->get_best(timestamp, &best_idx);
276  assert(new_best_val == best_data[0]);
277  //the new best sensor should now be the sensor with the higher priority
278  assert(best_idx == val2_idx);
279  //should not have detected a real failover
280  assert(0 == group->failover_count());
281 
282  delete group; //cleanup
283 }
284 
285 /**
286  * Verify that the DataGroupValidator will prefer a sensor with no errors over a sensor with high errors
287  */
289 {
290  unsigned num_siblings = 0;
291  DataValidator *validator1 = nullptr;
292  DataValidator *validator2 = nullptr;
293 
294  uint64_t timestamp = base_timestamp;
295 
296  DataValidatorGroup *group = setup_group_with_two_validator_handles(&validator1, &validator2, &num_siblings);
297  //printf("num_siblings: %d \n",num_siblings);
298  int val1_idx = (int)num_siblings - 2;
299  int val2_idx = (int)num_siblings - 1;
300 
301 
302  fill_two_with_valid_data(group, val1_idx, val2_idx, 100);
303 
304  int best_idx = -1;
305  float *best_data = nullptr;
306 
307  //trigger a real failover
308  float new_best_val = 3.14159f;
309  float data[DataValidator::dimensions] = {new_best_val};
310  //trigger a bunch of errors on the previous best sensor
311  unsigned val1_err_count = 0;
312 
313  for (int i = 0; i < 25; i++) {
314  group->put(val1_idx, timestamp, data, ++val1_err_count, 100);
315  group->put(val2_idx, timestamp, data, 0, 10);
316  }
317 
318  assert(validator1->error_count() == val1_err_count);
319 
320  //since validator1 is experiencing errors, we should see a failover to validator2
321  best_data = group->get_best(timestamp + 1, &best_idx);
322  assert(nullptr != best_data);
323  assert(new_best_val == best_data[0]);
324  assert(best_idx == val2_idx);
325  //should have detected a real failover
326  printf("failover_count: %d \n", group->failover_count());
327  assert(1 == group->failover_count());
328 
329  //even though validator1 has encountered a bunch of errors, it hasn't failed
330  assert(DataValidator::ERROR_FLAG_NO_ERROR == validator1->state());
331 
332  // although we failed over from one sensor to another, this is not the same thing tracked by failover_index
333  int fail_idx = group->failover_index();
334  assert(-1 == fail_idx);//no failed sensor
335 
336  //since no sensor has actually hard-failed, the group failover state is NO_ERROR
338 
339 
340  delete group; //cleanup
341 }
342 
343 /**
344  * Verify that we get expected vibration values after injecting samples.
345  */
347 {
348  unsigned num_siblings = 0;
349  uint64_t timestamp = base_timestamp;
350 
351  DataValidatorGroup *group = setup_base_group(&num_siblings);
352 
353  //now we add validators
354  DataValidator *validator = add_validator_to_group(group);
355  assert(nullptr != validator);
356  num_siblings++;
357  float *vibes = validator->vibration_offset();
358  assert(nullptr != vibes);
359  //printf("val vibes: %f \n", vibes[0]);
360  //should be no vibration data yet
361  assert(0 == vibes[0]);
362 
363  float vibe_o = group->get_vibration_offset(timestamp, 0);
364  //printf("group vibe_o %f \n", vibe_o);
365  // there should be no known vibe offset before samples are inserted
366  assert(-1.0f == vibe_o);
367 
368  float rms_err = 0.0f;
369  //insert some swinging values
370  insert_values_around_mean(validator, 3.14159f, 1000, &rms_err, &timestamp);
371  vibes = validator->vibration_offset();
372  assert(nullptr != vibes);
373  printf("val1 vibes: %f rms_err: %f \n", vibes[0], (double)rms_err);
374 
375  vibe_o = group->get_vibration_offset(timestamp, 0);
376  printf("group vibe_o %f \n", vibe_o);
377  //the one validator's vibration offset should match the group's vibration offset
378  assert(vibes[0] == vibe_o);
379 
380  //this should be "The best RMS value of a non-timed out sensor"
381  float group_vibe_fact = group->get_vibration_factor(timestamp);
382  float val1_rms = (validator->rms())[0];
383  printf("group_vibe_fact: %f val1_rms: %f\n", (double)group_vibe_fact, (double)val1_rms);
384  assert(group_vibe_fact == val1_rms);
385 
386 }
387 
388 /**
389  * Force once sensor to fail and ensure that we detect it
390  */
392 {
393  unsigned num_siblings = 0;
394  uint64_t timestamp = base_timestamp;
395  const float sufficient_incr_value = (1.1f * 1E-6f);
396  const uint32_t timeout_usec = 2000;//derived from class-private value
397 
398  float val = 3.14159f;
399 
400  DataValidatorGroup *group = setup_base_group(&num_siblings);
401 
402  //now we add validators
403  DataValidator *validator = add_validator_to_group(group);
404  assert(nullptr != validator);
405  num_siblings++;
406  int val_idx = num_siblings - 1;
407 
408  fill_validator_with_samples(validator, sufficient_incr_value, &val, &timestamp);
409  //the best should now be the one validator we've filled with samples
410 
411  int best_idx = -1;
412  float *best_data = group->get_best(timestamp, &best_idx);
413  assert(nullptr != best_data);
414  //printf("best_idx: %d val_idx: %d\n", best_idx, val_idx);
415  assert(best_idx == val_idx);
416 
417  //now force a timeout failure in the one validator, by checking confidence long past timeout
418  validator->confidence(timestamp + (1.1 * timeout_usec));
420 
421  //now that the one sensor has failed, the group should detect this as well
422  int fail_idx = group->failover_index();
423  assert(val_idx == fail_idx);
424 
425  delete group;
426 }
427 
428 int main(int argc, char *argv[])
429 {
430  (void)argc; // unused
431  (void)argv; // unused
432 
433  test_init();
434  test_put();
437  test_vibration();
439 
440  return 0; //passed
441 }
uint32_t get_timeout() const
Get the timeout value.
DataValidatorGroup * setup_base_group(unsigned *sibling_count)
Initialize a DataValidatorGroup with some common settings;.
static constexpr uint32_t ERROR_FLAG_NO_ERROR
Data validator error states.
void test_sensor_failure()
Force once sensor to fail and ensure that we detect it.
DataValidatorGroup * setup_group_with_two_validator_handles(DataValidator **validator1_handle, DataValidator **validator2_handle, unsigned *sibling_count)
Create a DataValidatorGroup and tack on two additional DataValidators.
void put(unsigned index, uint64_t timestamp, const float val[3], uint64_t error_count, int priority)
Put an item into the validator group.
void set_timeout(uint32_t timeout_interval_us)
Set the timeout value for the whole group.
DataValidator * add_validator_to_group(DataValidatorGroup *group)
Dynamically add a validator to the group after construction.
const uint32_t base_timeout_usec
static constexpr uint32_t ERROR_FLAG_TIMEOUT
int main(int argc, char *argv[])
int failover_index()
Get the index of the failed sensor in the group.
void test_vibration()
Verify that we get expected vibration values after injecting samples.
const uint64_t base_timestamp
void print()
Print the validator value.
uint32_t failover_state()
Get the error state of the failed sensor in the group.
const unsigned base_num_siblings
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
float * get_best(uint64_t timestamp, int *index)
Get the best data triplet of the group.
void test_init()
float get_vibration_offset(uint64_t timestamp, int axis)
Get the vibration offset in the sensor unit.
float get_vibration_factor(uint64_t timestamp)
Get the RMS / vibration factor.
void test_simple_failover()
Verify that the DataGroupValidator will prefer a sensor with no errors over a sensor with high errors...
void test_put()
Happy path test of put method – ensure the "best" sensor selected is the one with highest priority...
uint8_t * data
Definition: dataman.cpp:149
void set_equal_value_threshold(uint32_t threshold)
Set the equal count threshold for the whole group.
DataValidator * add_new_validator()
Create a new Validator (with index equal to the number of currently existing validators) ...
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
Definition: integration.cpp:8
A data validation group to identify anomalies in data streams.
uint32_t state() const
Get the error state of this validator.
float * rms()
Get the RMS values of this validator.
float * value()
Get the values of this validator.
unsigned failover_count() const
Get the number of failover events.
void set_equal_value_threshold(uint32_t threshold)
Set the equal count threshold.
void fill_one_with_valid_data(DataValidatorGroup *group, int val1_idx, uint32_t num_samples)
Fill one DataValidator with samples, by index.
float * vibration_offset()
Get the vibration offset.
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.
void fill_two_with_valid_data(DataValidatorGroup *group, int val1_idx, int val2_idx, uint32_t num_samples)
Fill two validators in the group with samples, by index.
A data validation class to identify anomalies in data streams.
float confidence(uint64_t timestamp)
Get the confidence of this validator.
uint64_t error_count() const
Get the error count of this validator.
static const unsigned dimensions
void test_priority_switch()
Verify that the DataValidatorGroup will select the sensor with the latest higher priority as "best"...