From 8da3f1f8f99b5b01e40312c61722883b909e35f6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 30 Aug 2015 18:47:21 +0200 Subject: [PATCH] data validator: Move implementations to CPP files --- src/lib/ecl/module.mk | 4 +- src/lib/ecl/validation/data_validator.cpp | 135 ++++++++++++ src/lib/ecl/validation/data_validator.h | 96 --------- .../ecl/validation/data_validator_group.cpp | 192 ++++++++++++++++++ src/lib/ecl/validation/data_validator_group.h | 152 -------------- 5 files changed, 330 insertions(+), 249 deletions(-) create mode 100644 src/lib/ecl/validation/data_validator.cpp create mode 100644 src/lib/ecl/validation/data_validator_group.cpp diff --git a/src/lib/ecl/module.mk b/src/lib/ecl/module.mk index 03974c9503..747a7c5be0 100644 --- a/src/lib/ecl/module.mk +++ b/src/lib/ecl/module.mk @@ -39,6 +39,8 @@ SRCS = attitude_fw/ecl_controller.cpp \ attitude_fw/ecl_pitch_controller.cpp \ attitude_fw/ecl_roll_controller.cpp \ attitude_fw/ecl_yaw_controller.cpp \ - l1/ecl_l1_pos_controller.cpp + l1/ecl_l1_pos_controller.cpp \ + validation/data_validator.cpp \ + validation/data_validator_group.cpp MAXOPTIMIZATION = -Os diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp new file mode 100644 index 0000000000..992a1018b7 --- /dev/null +++ b/src/lib/ecl/validation/data_validator.cpp @@ -0,0 +1,135 @@ +DataValidator::DataValidator(DataValidator *prev_sibling) : + _time_last(0), + _timeout_interval(70000), + _event_count(0), + _error_count(0), + _mean{0.0f}, + _lp{0.0f}, + _M2{0.0f}, + _rms{0.0f}, + _value{0.0f}, + _value_equal_count(0), + _sibling(prev_sibling) +{ + +} + +DataValidator::~DataValidator() +{ + +} + +void +DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in) +{ + _event_count++; + _error_count = error_count_in; + + for (unsigned i = 0; i < _dimensions; i++) { + if (_time_last == 0) { + _mean[i] = 0; + _lp[i] = val[i]; + _M2[i] = 0; + } else { + float lp_val = val[i] - _lp[i]; + + float delta_val = lp_val - _mean[i]; + _mean[i] += delta_val / _event_count; + _M2[i] += delta_val * (lp_val - _mean[i]); + _rms[i] = sqrtf(_M2[i] / (_event_count - 1)); + + if (fabsf(_value[i] - val[i]) < 0.000001f) { + _value_equal_count++; + } else { + _value_equal_count = 0; + } + } + + // XXX replace with better filter, make it auto-tune to update rate + _lp[i] = _lp[i] * 0.5f + val[i] * 0.5f; + + _value[i] = val[i]; + } + + _time_last = timestamp; +} +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator.c + * + * A data validation class to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +float +DataValidator::confidence(uint64_t timestamp) +{ + /* check if we have any data */ + if (_time_last == 0) { + return 0.0f; + } + + /* check error count limit */ + if (_error_count > NORETURN_ERRCOUNT) { + return 0.0f; + } + + /* we got the exact same sensor value N times in a row */ + if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { + return 0.0f; + } + + /* timed out - that's it */ + if (timestamp - _time_last > _timeout_interval) { + return 0.0f; + } + + return 1.0f; +} + +void +DataValidator::print() +{ + if (_time_last == 0) { + printf("\tno data\n"); + return; + } + + for (unsigned i = 0; i < _dimensions; i++) { + printf("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n", + (double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]); + } +} diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h index f768c278bd..dfd88b6e42 100644 --- a/src/lib/ecl/validation/data_validator.h +++ b/src/lib/ecl/validation/data_validator.h @@ -119,99 +119,3 @@ private: DataValidator(const DataValidator&); DataValidator operator=(const DataValidator&); }; - -DataValidator::DataValidator(DataValidator *prev_sibling) : - _time_last(0), - _timeout_interval(70000), - _event_count(0), - _error_count(0), - _mean{0.0f}, - _lp{0.0f}, - _M2{0.0f}, - _rms{0.0f}, - _value{0.0f}, - _value_equal_count(0), - _sibling(prev_sibling) -{ - -} - -DataValidator::~DataValidator() -{ - -} - -void -DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in) -{ - _event_count++; - _error_count = error_count_in; - - for (unsigned i = 0; i < _dimensions; i++) { - if (_time_last == 0) { - _mean[i] = 0; - _lp[i] = val[i]; - _M2[i] = 0; - } else { - float lp_val = val[i] - _lp[i]; - - float delta_val = lp_val - _mean[i]; - _mean[i] += delta_val / _event_count; - _M2[i] += delta_val * (lp_val - _mean[i]); - _rms[i] = sqrtf(_M2[i] / (_event_count - 1)); - - if (fabsf(_value[i] - val[i]) < 0.000001f) { - _value_equal_count++; - } else { - _value_equal_count = 0; - } - } - - // XXX replace with better filter, make it auto-tune to update rate - _lp[i] = _lp[i] * 0.5f + val[i] * 0.5f; - - _value[i] = val[i]; - } - - _time_last = timestamp; -} - -float -DataValidator::confidence(uint64_t timestamp) -{ - /* check if we have any data */ - if (_time_last == 0) { - return 0.0f; - } - - /* check error count limit */ - if (_error_count > NORETURN_ERRCOUNT) { - return 0.0f; - } - - /* we got the exact same sensor value N times in a row */ - if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) { - return 0.0f; - } - - /* timed out - that's it */ - if (timestamp - _time_last > _timeout_interval) { - return 0.0f; - } - - return 1.0f; -} - -void -DataValidator::print() -{ - if (_time_last == 0) { - printf("\tno data\n"); - return; - } - - for (unsigned i = 0; i < _dimensions; i++) { - printf("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f\n", - (double) _value[i], (double)_lp[i], (double)_mean[i], (double)_rms[i]); - } -} diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp new file mode 100644 index 0000000000..5e3232b701 --- /dev/null +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -0,0 +1,192 @@ +/**************************************************************************** + * + * Copyright (c) 2015 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file data_validator_group.cpp + * + * A data validation group to identify anomalies in data streams + * + * @author Lorenz Meier + */ + +DataValidatorGroup::DataValidatorGroup(unsigned siblings) : + _first(nullptr), + _curr_best(-1), + _prev_best(-1), + _first_failover_time(0), + _toggle_count(0) +{ + DataValidator *next = _first; + + for (unsigned i = 0; i < siblings; i++) { + next = new DataValidator(next); + } + + _first = next; +} + +DataValidatorGroup::~DataValidatorGroup() +{ + +} + +void +DataValidatorGroup::set_timeout(uint64_t timeout_interval_us) +{ + DataValidator *next = _first; + + while (next != nullptr) { + next->set_timeout(timeout_interval_us); + next = next->sibling(); + } +} + +void +DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count) +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (i == index) { + next->put(timestamp, val, error_count); + break; + } + next = next->sibling(); + i++; + } +} + +float* +DataValidatorGroup::get_best(uint64_t timestamp, int *index) +{ + DataValidator *next = _first; + + // XXX This should eventually also include voting + int pre_check_best = _curr_best; + float max_confidence = -1.0f; + int max_index = -1; + uint64_t min_error_count = 30000; + DataValidator *best = nullptr; + + unsigned i = 0; + + while (next != nullptr) { + float confidence = next->confidence(timestamp); + if (confidence > max_confidence || + (fabsf(confidence - max_confidence) < 0.01f && next->error_count() < min_error_count)) { + max_index = i; + max_confidence = confidence; + min_error_count = next->error_count(); + best = next; + } + + next = next->sibling(); + i++; + } + + /* the current best sensor is not matching the previous best sensor */ + if (max_index != _curr_best) { + + /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ + if (_curr_best < 0) { + _prev_best = max_index; + } else { + /* we were initialized before, this is a real failsafe */ + _prev_best = pre_check_best; + _toggle_count++; + + /* if this is the first time, log when we failed */ + if (_first_failover_time == 0) { + _first_failover_time = timestamp; + } + } + + /* for all cases we want to keep a record of the best index */ + _curr_best = max_index; + } + *index = max_index; + return (best) ? best->value() : nullptr; +} + +float +DataValidatorGroup::get_vibration_factor(uint64_t timestamp) +{ + DataValidator *next = _first; + + float vibe = 0.0f; + + /* find the best RMS value of a non-timed out sensor */ + while (next != nullptr) { + + if (next->confidence(timestamp) > 0.5f) { + float* rms = next->rms(); + + for (unsigned j = 0; j < 3; j++) { + if (rms[j] > vibe) { + vibe = rms[j]; + } + } + } + + next = next->sibling(); + } + + return vibe; +} + +void +DataValidatorGroup::print() +{ + /* print the group's state */ + warnx("validator: best: %d, prev best: %d, failsafe: %s (# %u)", + _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", + _toggle_count); + + + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + printf("sensor #%u:\n", i); + next->print(); + next = next->sibling(); + i++; + } +} + +unsigned +DataValidatorGroup::failover_count() +{ + return _toggle_count; +} diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h index e67f878eb8..dbbe3bdce1 100644 --- a/src/lib/ecl/validation/data_validator_group.h +++ b/src/lib/ecl/validation/data_validator_group.h @@ -103,155 +103,3 @@ private: DataValidatorGroup(const DataValidatorGroup&); DataValidatorGroup operator=(const DataValidatorGroup&); }; - -DataValidatorGroup::DataValidatorGroup(unsigned siblings) : - _first(nullptr), - _curr_best(-1), - _prev_best(-1), - _first_failover_time(0), - _toggle_count(0) -{ - DataValidator *next = _first; - - for (unsigned i = 0; i < siblings; i++) { - next = new DataValidator(next); - } - - _first = next; -} - -DataValidatorGroup::~DataValidatorGroup() -{ - -} - -void -DataValidatorGroup::set_timeout(uint64_t timeout_interval_us) -{ - DataValidator *next = _first; - - while (next != nullptr) { - next->set_timeout(timeout_interval_us); - next = next->sibling(); - } -} - -void -DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count) -{ - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - if (i == index) { - next->put(timestamp, val, error_count); - break; - } - next = next->sibling(); - i++; - } -} - -float* -DataValidatorGroup::get_best(uint64_t timestamp, int *index) -{ - DataValidator *next = _first; - - // XXX This should eventually also include voting - int pre_check_best = _curr_best; - float max_confidence = -1.0f; - int max_index = -1; - uint64_t min_error_count = 30000; - DataValidator *best = nullptr; - - unsigned i = 0; - - while (next != nullptr) { - float confidence = next->confidence(timestamp); - if (confidence > max_confidence || - (fabsf(confidence - max_confidence) < 0.01f && next->error_count() < min_error_count)) { - max_index = i; - max_confidence = confidence; - min_error_count = next->error_count(); - best = next; - } - - next = next->sibling(); - i++; - } - - /* the current best sensor is not matching the previous best sensor */ - if (max_index != _curr_best) { - - /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ - if (_curr_best < 0) { - _prev_best = max_index; - } else { - /* we were initialized before, this is a real failsafe */ - _prev_best = pre_check_best; - _toggle_count++; - - /* if this is the first time, log when we failed */ - if (_first_failover_time == 0) { - _first_failover_time = timestamp; - } - } - - /* for all cases we want to keep a record of the best index */ - _curr_best = max_index; - } - *index = max_index; - return (best) ? best->value() : nullptr; -} - -float -DataValidatorGroup::get_vibration_factor(uint64_t timestamp) -{ - DataValidator *next = _first; - - float vibe = 0.0f; - - /* find the best RMS value of a non-timed out sensor */ - while (next != nullptr) { - - if (next->confidence(timestamp) > 0.5f) { - float* rms = next->rms(); - - for (unsigned j = 0; j < 3; j++) { - if (rms[j] > vibe) { - vibe = rms[j]; - } - } - } - - next = next->sibling(); - } - - return vibe; -} - -void -DataValidatorGroup::print() -{ - /* print the group's state */ - warnx("validator: best: %d, prev best: %d, failsafe: %s (# %u)", - _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", - _toggle_count); - - - DataValidator *next = _first; - unsigned i = 0; - - while (next != nullptr) { - printf("sensor #%u:\n", i); - next->print(); - next = next->sibling(); - i++; - } -} - -unsigned -DataValidatorGroup::failover_count() -{ - return _toggle_count; -}