mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-05-13 17:57:35 +08:00
Initial import
This commit is contained in:
Lorenz Meier
@@ -0,0 +1,162 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#include "data_validator.h"
|
||||
#include <ecl/ecl.h>
|
||||
|
||||
DataValidator::DataValidator(DataValidator *prev_sibling) :
|
||||
_time_last(0),
|
||||
_timeout_interval(20000),
|
||||
_event_count(0),
|
||||
_error_count(0),
|
||||
_error_density(0),
|
||||
_priority(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, 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;
|
||||
|
||||
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;
|
||||
}
|
||||
|
||||
/* 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
|
||||
DataValidator::priority()
|
||||
{
|
||||
return _priority;
|
||||
}
|
||||
|
||||
void
|
||||
DataValidator::print()
|
||||
{
|
||||
if (_time_last == 0) {
|
||||
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 conf: %8.4f",
|
||||
(double) _value[i], (double)_lp[i], (double)_mean[i],
|
||||
(double)_rms[i], (double)confidence(hrt_absolute_time()));
|
||||
}
|
||||
}
|
||||
@@ -0,0 +1,137 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.h
|
||||
*
|
||||
* A data validation class to identify anomalies in data streams
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include <cmath>
|
||||
#include <stdint.h>
|
||||
|
||||
class __EXPORT DataValidator {
|
||||
public:
|
||||
DataValidator(DataValidator *prev_sibling = nullptr);
|
||||
virtual ~DataValidator();
|
||||
|
||||
/**
|
||||
* Put an item into the validator.
|
||||
*
|
||||
* @param val Item to put
|
||||
*/
|
||||
void put(uint64_t timestamp, float val[3], uint64_t error_count, int priority);
|
||||
|
||||
/**
|
||||
* Get the next sibling in the group
|
||||
*
|
||||
* @return the next sibling
|
||||
*/
|
||||
DataValidator* sibling() { return _sibling; }
|
||||
|
||||
/**
|
||||
* Get the confidence of this validator
|
||||
* @return the confidence between 0 and 1
|
||||
*/
|
||||
float confidence(uint64_t timestamp);
|
||||
|
||||
/**
|
||||
* Get the error count of this validator
|
||||
* @return the error count
|
||||
*/
|
||||
uint64_t error_count() { return _error_count; }
|
||||
|
||||
/**
|
||||
* Get the values of this validator
|
||||
* @return the stored value
|
||||
*/
|
||||
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
|
||||
*/
|
||||
int priority();
|
||||
|
||||
/**
|
||||
* Get the RMS values of this validator
|
||||
* @return the stored RMS
|
||||
*/
|
||||
float* rms() { return _rms; }
|
||||
|
||||
/**
|
||||
* Print the validator value
|
||||
*
|
||||
*/
|
||||
void print();
|
||||
|
||||
/**
|
||||
* Set the timeout value
|
||||
*
|
||||
* @param timeout_interval_us The timeout interval in microseconds
|
||||
*/
|
||||
void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
|
||||
|
||||
private:
|
||||
static const unsigned _dimensions = 3;
|
||||
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 */
|
||||
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 */
|
||||
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 = 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 */
|
||||
DataValidator(const DataValidator&);
|
||||
DataValidator operator=(const DataValidator&);
|
||||
};
|
||||
@@ -0,0 +1,224 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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 <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#include "data_validator_group.h"
|
||||
#include <ecl/ecl.h>
|
||||
|
||||
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, int priority)
|
||||
{
|
||||
DataValidator *next = _first;
|
||||
unsigned i = 0;
|
||||
|
||||
while (next != nullptr) {
|
||||
if (i == index) {
|
||||
next->put(timestamp, val, error_count, priority);
|
||||
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 pre_check_confidence = 1.0f;
|
||||
int pre_check_prio = -1;
|
||||
float max_confidence = -1.0f;
|
||||
int max_priority = -1000;
|
||||
int max_index = -1;
|
||||
DataValidator *best = nullptr;
|
||||
|
||||
unsigned i = 0;
|
||||
|
||||
while (next != nullptr) {
|
||||
float confidence = next->confidence(timestamp);
|
||||
|
||||
if (static_cast<int>(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();
|
||||
best = next;
|
||||
}
|
||||
|
||||
next = next->sibling();
|
||||
i++;
|
||||
}
|
||||
|
||||
/* 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;
|
||||
|
||||
if (true_failsafe) {
|
||||
_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 */
|
||||
ECL_INFO("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) {
|
||||
if (next->used()) {
|
||||
ECL_INFO("sensor #%u, prio: %d", i, next->priority());
|
||||
next->print();
|
||||
}
|
||||
next = next->sibling();
|
||||
i++;
|
||||
}
|
||||
}
|
||||
|
||||
unsigned
|
||||
DataValidatorGroup::failover_count()
|
||||
{
|
||||
return _toggle_count;
|
||||
}
|
||||
@@ -0,0 +1,108 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* 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.h
|
||||
*
|
||||
* A data validation group to identify anomalies in data streams
|
||||
*
|
||||
* @author Lorenz Meier <lorenz@px4.io>
|
||||
*/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "data_validator.h"
|
||||
|
||||
class __EXPORT DataValidatorGroup {
|
||||
public:
|
||||
DataValidatorGroup(unsigned siblings);
|
||||
virtual ~DataValidatorGroup();
|
||||
|
||||
/**
|
||||
* Put an item into the validator group.
|
||||
*
|
||||
* @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, int priority);
|
||||
|
||||
/**
|
||||
* Get the best data triplet of the group
|
||||
*
|
||||
* @return pointer to the array of best values
|
||||
*/
|
||||
float* get_best(uint64_t timestamp, int *index);
|
||||
|
||||
/**
|
||||
* Get the RMS / vibration factor
|
||||
*
|
||||
* @return float value representing the RMS, which a valid indicator for vibration
|
||||
*/
|
||||
float get_vibration_factor(uint64_t timestamp);
|
||||
|
||||
/**
|
||||
* Get the number of failover events
|
||||
*
|
||||
* @return the number of failovers
|
||||
*/
|
||||
unsigned failover_count();
|
||||
|
||||
/**
|
||||
* Print the validator value
|
||||
*
|
||||
*/
|
||||
void print();
|
||||
|
||||
/**
|
||||
* Set the timeout value
|
||||
*
|
||||
* @param timeout_interval_us The timeout interval in microseconds
|
||||
*/
|
||||
void set_timeout(uint64_t timeout_interval_us);
|
||||
|
||||
private:
|
||||
DataValidator *_first; /**< sibling in the group */
|
||||
int _curr_best; /**< currently best index */
|
||||
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&);
|
||||
DataValidatorGroup operator=(const DataValidatorGroup&);
|
||||
};
|
||||
Reference in New Issue
Block a user