From 526bb51570b5e90881fd2884c3e83d3de65b7f06 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 3 Oct 2015 15:24:24 +0200 Subject: [PATCH] Validation: Use error density instead of error count. Fail over to higher priority sensors once they become available. --- src/lib/ecl/validation/data_validator.cpp | 25 ++++++++-- src/lib/ecl/validation/data_validator.h | 10 +++- .../ecl/validation/data_validator_group.cpp | 49 ++++++++++++++----- src/lib/ecl/validation/data_validator_group.h | 1 + 4 files changed, 67 insertions(+), 18 deletions(-) diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp index 037cb88645..737d9ea1a3 100644 --- a/src/lib/ecl/validation/data_validator.cpp +++ b/src/lib/ecl/validation/data_validator.cpp @@ -44,9 +44,10 @@ DataValidator::DataValidator(DataValidator *prev_sibling) : _time_last(0), - _timeout_interval(70000), + _timeout_interval(20000), _event_count(0), _error_count(0), + _error_density(0), _priority(0), _mean{0.0f}, _lp{0.0f}, @@ -68,6 +69,13 @@ void DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in) { _event_count++; + + if (error_count_in > _error_count) { + _error_density += (error_count_in - _error_count); + } else if (_error_density > 0) { + _error_density--; + } + _error_count = error_count_in; _priority = priority_in; @@ -123,7 +131,13 @@ DataValidator::confidence(uint64_t timestamp) return 0.0f; } - return 1.0f; + /* cap error density counter at window size */ + if (_error_density > ERROR_DENSITY_WINDOW) { + _error_density = ERROR_DENSITY_WINDOW; + } + + /* return local error density for last N measurements */ + return 1.0f - (_error_density / ERROR_DENSITY_WINDOW); } int @@ -136,12 +150,13 @@ void DataValidator::print() { if (_time_last == 0) { - ECL_INFO("\tno data\n"); + ECL_INFO("\tno data"); return; } for (unsigned i = 0; i < _dimensions; i++) { - ECL_INFO("\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]); + ECL_INFO("\tval: %8.4f, lp: %8.4f mean dev: %8.4f RMS: %8.4f conf: %8.4f", + (double) _value[i], (double)_lp[i], (double)_mean[i], + (double)_rms[i], (double)confidence(hrt_absolute_time())); } } diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h index 686a810c4e..dde9cb51aa 100644 --- a/src/lib/ecl/validation/data_validator.h +++ b/src/lib/ecl/validation/data_validator.h @@ -81,6 +81,12 @@ public: */ float* value() { return _value; } + /** + * Get the used status of this validator + * @return true if this validator ever saw data + */ + bool used() { return (_time_last > 0); } + /** * Get the priority of this validator * @return the stored priority @@ -112,6 +118,7 @@ private: uint64_t _timeout_interval; /**< interval in which the datastream times out in us */ uint64_t _event_count; /**< total data counter */ uint64_t _error_count; /**< error count */ + int _error_density; /**< ratio between successful reads and errors */ int _priority; /**< sensor nominal priority */ float _mean[_dimensions]; /**< mean of value */ float _lp[3]; /**< low pass value */ @@ -120,7 +127,8 @@ private: float _value[3]; /**< last value */ float _value_equal_count; /**< equal values in a row */ DataValidator *_sibling; /**< sibling in the group */ - const unsigned NORETURN_ERRCOUNT = 100; /**< if the error count reaches this value, return sensor as invalid */ + const unsigned NORETURN_ERRCOUNT = 10000; /**< if the error count reaches this value, return sensor as invalid */ + const float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */ const unsigned VALUE_EQUAL_COUNT_MAX = 100; /**< if the sensor value is the same (accumulated also between axes) this many times, flag it */ /* we don't want this class to be copied */ diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index cdd1f6a677..e1ba53333e 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -97,24 +97,35 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) // XXX This should eventually also include voting int pre_check_best = _curr_best; + float pre_check_confidence = 1.0f; + int pre_check_prio = -1; float max_confidence = -1.0f; int max_priority = -1000; 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) && - (next->priority() >= max_priority)))) { + + if (i == pre_check_best) { + pre_check_prio = next->priority(); + pre_check_confidence = confidence; + } + + /* + * Switch if: + * 1) the confidence is higher and priority is equal or higher + * 2) the confidence is no less than 1% different and the priority is higher + */ + if (((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) || + (confidence > max_confidence && (next->priority() >= max_priority)) || + (fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority)) + ) { max_index = i; max_confidence = confidence; max_priority = next->priority(); - min_error_count = next->error_count(); best = next; } @@ -125,17 +136,29 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) /* the current best sensor is not matching the previous best sensor */ if (max_index != _curr_best) { + bool true_failsafe = true; + + /* check wether the switch was a failsafe or preferring a higher priority sensor */ + if (pre_check_prio != -1 && pre_check_prio < max_priority && + fabsf(pre_check_confidence - max_confidence) < 0.1f) { + /* this is not a failover */ + true_failsafe = false; + } + /* 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; + if (true_failsafe) { + _toggle_count++; + + /* if this is the first time, log when we failed */ + if (_first_failover_time == 0) { + _first_failover_time = timestamp; + } } } @@ -185,8 +208,10 @@ DataValidatorGroup::print() unsigned i = 0; while (next != nullptr) { - ECL_INFO("sensor #%u:\n", i); - next->print(); + if (next->used()) { + ECL_INFO("sensor #%u, prio: %d", i, next->priority()); + next->print(); + } next = next->sibling(); i++; } diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h index f35e0d71c1..3756be2638 100644 --- a/src/lib/ecl/validation/data_validator_group.h +++ b/src/lib/ecl/validation/data_validator_group.h @@ -100,6 +100,7 @@ private: int _prev_best; /**< the previous best index */ uint64_t _first_failover_time; /**< timestamp where the first failover occured or zero if none occured */ unsigned _toggle_count; /**< number of back and forth switches between two sensors */ + static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f; /* we don't want this class to be copied */ DataValidatorGroup(const DataValidatorGroup&);