From ae56496ba3e96a431f3e3b54d85d7bedaf63a92d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 7 Sep 2015 09:57:42 +0200 Subject: [PATCH] Data validator: Add priority support --- src/lib/ecl/validation/data_validator.cpp | 10 +++++++++- src/lib/ecl/validation/data_validator.h | 13 ++++++++++--- src/lib/ecl/validation/data_validator_group.cpp | 10 +++++++--- src/lib/ecl/validation/data_validator_group.h | 10 ++++++---- 4 files changed, 32 insertions(+), 11 deletions(-) diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp index 810ab7f6ff..037cb88645 100644 --- a/src/lib/ecl/validation/data_validator.cpp +++ b/src/lib/ecl/validation/data_validator.cpp @@ -47,6 +47,7 @@ DataValidator::DataValidator(DataValidator *prev_sibling) : _timeout_interval(70000), _event_count(0), _error_count(0), + _priority(0), _mean{0.0f}, _lp{0.0f}, _M2{0.0f}, @@ -64,10 +65,11 @@ DataValidator::~DataValidator() } void -DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in) +DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, int priority_in) { _event_count++; _error_count = error_count_in; + _priority = priority_in; for (unsigned i = 0; i < _dimensions; i++) { if (_time_last == 0) { @@ -124,6 +126,12 @@ DataValidator::confidence(uint64_t timestamp) return 1.0f; } +int +DataValidator::priority() +{ + return _priority; +} + void DataValidator::print() { diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h index efcec93745..686a810c4e 100644 --- a/src/lib/ecl/validation/data_validator.h +++ b/src/lib/ecl/validation/data_validator.h @@ -54,7 +54,7 @@ public: * * @param val Item to put */ - void put(uint64_t timestamp, float val[3], uint64_t error_count); + void put(uint64_t timestamp, float val[3], uint64_t error_count, int priority); /** * Get the next sibling in the group @@ -81,6 +81,12 @@ public: */ float* value() { return _value; } + /** + * Get the priority of this validator + * @return the stored priority + */ + int priority(); + /** * Get the RMS values of this validator * @return the stored RMS @@ -106,14 +112,15 @@ 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 */ - float _mean[_dimensions]; /**< mean of value */ + int _priority; /**< sensor nominal priority */ + float _mean[_dimensions]; /**< mean of value */ float _lp[3]; /**< low pass value */ float _M2[3]; /**< RMS component value */ float _rms[3]; /**< root mean square error */ float _value[3]; /**< last value */ float _value_equal_count; /**< equal values in a row */ DataValidator *_sibling; /**< sibling in the group */ - const unsigned NORETURN_ERRCOUNT = 1000; /**< if the error count reaches this value, return sensor as invalid */ + const unsigned NORETURN_ERRCOUNT = 100; /**< if the error count reaches this value, return sensor as invalid */ 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 25002de0f6..cdd1f6a677 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -75,14 +75,14 @@ DataValidatorGroup::set_timeout(uint64_t timeout_interval_us) } void -DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count) +DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority) { DataValidator *next = _first; unsigned i = 0; while (next != nullptr) { if (i == index) { - next->put(timestamp, val, error_count); + next->put(timestamp, val, error_count, priority); break; } next = next->sibling(); @@ -98,6 +98,7 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) // XXX This should eventually also include voting int pre_check_best = _curr_best; float max_confidence = -1.0f; + int max_priority = -1000; int max_index = -1; uint64_t min_error_count = 30000; DataValidator *best = nullptr; @@ -107,9 +108,12 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) while (next != nullptr) { float confidence = next->confidence(timestamp); if (confidence > max_confidence || - (fabsf(confidence - max_confidence) < 0.01f && next->error_count() < min_error_count)) { + (fabsf(confidence - max_confidence) < 0.01f && + ((next->error_count() < min_error_count) && + (next->priority() >= max_priority)))) { max_index = i; max_confidence = confidence; + max_priority = next->priority(); min_error_count = next->error_count(); best = next; } diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h index ec6162c606..f35e0d71c1 100644 --- a/src/lib/ecl/validation/data_validator_group.h +++ b/src/lib/ecl/validation/data_validator_group.h @@ -51,12 +51,14 @@ public: /** * Put an item into the validator group. * - * @param x X Item to put - * @param y Y Item to put - * @param z Z Item to put + * @param index Sensor index + * @param timestamp The timestamp of the measurement + * @param val The 3D vector + * @param error_count The current error count of the sensor + * @param priority The priority of the sensor */ void put(unsigned index, uint64_t timestamp, - float val[3], uint64_t error_count); + float val[3], uint64_t error_count, int priority); /** * Get the best data triplet of the group