dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

checked out validation code from master

This commit is contained in:
Roman 2015-12-19 08:53:08 +01:00
parent c58d1df7bc
commit dd171670ca
4 changed files with 115 additions and 30 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

57
validation/data_validator.cpp
View File

@ -43,6 +43,7 @@
#include <ecl/ecl.h>
DataValidator::DataValidator(DataValidator *prev_sibling) :
_error_mask(ERROR_FLAG_NO_ERROR),
_time_last(0),
_timeout_interval(20000),
_event_count(0),
@ -111,39 +112,45 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, in
float
DataValidator::confidence(uint64_t timestamp)
{
float ret = 1.0f;
/* 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;
}
_error_mask |= ERROR_FLAG_NO_DATA;
ret = 0.0f;
/* timed out - that's it */
if (timestamp - _time_last > _timeout_interval) {
return 0.0f;
}
} else if (timestamp - _time_last > _timeout_interval) {
_error_mask |= ERROR_FLAG_TIMEOUT;
ret = 0.0f;
/* we got the exact same sensor value N times in a row */
} else if (_value_equal_count > VALUE_EQUAL_COUNT_MAX) {
_error_mask |= ERROR_FLAG_STALE_DATA;
ret = 0.0f;
/* check error count limit */
} else if (_error_count > NORETURN_ERRCOUNT) {
_error_mask |= ERROR_FLAG_HIGH_ERRCOUNT;
ret = 0.0f;
/* cap error density counter at window size */
if (_error_density > ERROR_DENSITY_WINDOW) {
} else if (_error_density > ERROR_DENSITY_WINDOW) {
_error_mask |= ERROR_FLAG_HIGH_ERRDENSITY;
_error_density = ERROR_DENSITY_WINDOW;
/* no error */
} else {
_error_mask = ERROR_FLAG_NO_ERROR;
}
/* return local error density for last N measurements */
return 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
}
int
DataValidator::priority()
{
return _priority;
/* no critical errors */
if (ret > 0.0f) {
/* return local error density for last N measurements */
ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
}
return ret;
}
void

24
validation/data_validator.h
View File

@ -91,7 +91,18 @@ public:
* Get the priority of this validator
* @return the stored priority
*/
int priority();
int priority() { return (_priority); }
/**
* Get the error state of this validator
* @return the bitmask with the error status
*/
uint32_t state() { return _error_mask; }
/**
* Reset the error state of this validator
*/
void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; }
/**
* Get the RMS values of this validator
@ -111,9 +122,20 @@ public:
* @param timeout_interval_us The timeout interval in microseconds
*/
void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
/**
* Data validator error states
*/
static constexpr uint32_t ERROR_FLAG_NO_ERROR = (0x00000000U);
static constexpr uint32_t ERROR_FLAG_NO_DATA = (0x00000001U);
static constexpr uint32_t ERROR_FLAG_STALE_DATA = (0x00000001U << 1);
static constexpr uint32_t ERROR_FLAG_TIMEOUT = (0x00000001U << 2);
static constexpr uint32_t ERROR_FLAG_HIGH_ERRCOUNT = (0x00000001U << 3);
static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4);
private:
static const unsigned _dimensions = 3;
uint32_t _error_mask; /**< sensor error state */
uint64_t _time_last; /**< last timestamp */
uint64_t _timeout_interval; /**< interval in which the datastream times out in us */
uint64_t _event_count; /**< total data counter */

50
validation/data_validator_group.cpp
View File

@ -138,11 +138,13 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
bool true_failsafe = true;
/* check wether the switch was a failsafe or preferring a higher priority sensor */
/* check whether 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;
/* reset error flags, this is likely a hotplug sensor coming online late */
best->reset_state();
}
/* if we're no initialized, initialize the bookkeeping but do not count a failsafe */
@ -199,17 +201,25 @@ void
DataValidatorGroup::print()
{
/* print the group's state */
ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)",
ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)",
_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
_toggle_count);
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
if (next->used()) {
ECL_INFO("sensor #%u, prio: %d", i, next->priority());
uint32_t flags = next->state();
ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(),
((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""),
((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""),
((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""),
((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""),
((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : ""));
next->print();
}
next = next->sibling();
@ -222,3 +232,35 @@ DataValidatorGroup::failover_count()
{
return _toggle_count;
}
int
DataValidatorGroup::failover_index()
{
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
return i;
}
next = next->sibling();
i++;
}
return -1;
}
uint32_t
DataValidatorGroup::failover_state()
{
DataValidator *next = _first;
unsigned i = 0;
while (next != nullptr) {
if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) {
return next->state();
}
next = next->sibling();
i++;
}
return DataValidator::ERROR_FLAG_NO_ERROR;
}

14
validation/data_validator_group.h
View File

@ -80,6 +80,20 @@ public:
* @return the number of failovers
*/
unsigned failover_count();
/**
* Get the index of the failed sensor in the group
*
* @return index of the failed sensor
*/
int failover_index();
/**
* Get the error state of the failed sensor in the group
*
* @return bitmask with erro states of the failed sensor
*/
uint32_t failover_state();
/**
* Print the validator value