PX4 Firmware
PX4 Autopilot Software http://px4.io
|
#include <data_validator.h>
Public Member Functions | |
DataValidator ()=default | |
~DataValidator ()=default | |
void | put (uint64_t timestamp, float val, uint64_t error_count, int priority) |
Put an item into the validator. More... | |
void | put (uint64_t timestamp, const float val[dimensions], uint64_t error_count, int priority) |
Put a 3D item into the validator. More... | |
DataValidator * | sibling () |
Get the next sibling in the group. More... | |
void | setSibling (DataValidator *new_sibling) |
Set the sibling to the next node in the group. More... | |
float | confidence (uint64_t timestamp) |
Get the confidence of this validator. More... | |
uint64_t | error_count () const |
Get the error count of this validator. More... | |
float * | value () |
Get the values of this validator. More... | |
bool | used () const |
Get the used status of this validator. More... | |
int | priority () const |
Get the priority of this validator. More... | |
uint32_t | state () const |
Get the error state of this validator. More... | |
void | reset_state () |
Reset the error state of this validator. More... | |
float * | rms () |
Get the RMS values of this validator. More... | |
float * | vibration_offset () |
Get the vibration offset. More... | |
void | print () |
Print the validator value. More... | |
void | set_timeout (uint32_t timeout_interval_us) |
Set the timeout value. More... | |
void | set_equal_value_threshold (uint32_t threshold) |
Set the equal count threshold. More... | |
uint32_t | get_timeout () const |
Get the timeout value. More... | |
Static Public Attributes | |
static const unsigned | dimensions = 3 |
static constexpr uint32_t | ERROR_FLAG_NO_ERROR = (0x00000000U) |
Data validator error states. More... | |
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 Member Functions | |
DataValidator (const DataValidator &)=delete | |
DataValidator | operator= (const DataValidator &)=delete |
Private Attributes | |
uint32_t | _error_mask {ERROR_FLAG_NO_ERROR} |
sensor error state More... | |
uint32_t | _timeout_interval {20000} |
interval in which the datastream times out in us More... | |
uint64_t | _time_last {0} |
last timestamp More... | |
uint64_t | _event_count {0} |
total data counter More... | |
uint64_t | _error_count {0} |
error count More... | |
int | _error_density {0} |
ratio between successful reads and errors More... | |
int | _priority {0} |
sensor nominal priority More... | |
float | _mean [dimensions] {} |
mean of value More... | |
float | _lp [dimensions] {} |
low pass value More... | |
float | _M2 [dimensions] {} |
RMS component value. More... | |
float | _rms [dimensions] {} |
root mean square error More... | |
float | _value [dimensions] {} |
last value More... | |
float | _vibe [dimensions] {} |
vibration level, in sensor unit More... | |
unsigned | _value_equal_count {0} |
equal values in a row More... | |
unsigned | _value_equal_count_threshold {VALUE_EQUAL_COUNT_DEFAULT} |
when to consider an equal count as a problem More... | |
DataValidator * | _sibling {nullptr} |
sibling in the group More... | |
Static Private Attributes | |
static const constexpr unsigned | NORETURN_ERRCOUNT = 10000 |
if the error count reaches this value, return sensor as invalid More... | |
static const constexpr float | ERROR_DENSITY_WINDOW = 100.0f |
window in measurement counts for errors More... | |
static const constexpr unsigned | VALUE_EQUAL_COUNT_DEFAULT = 100 |
if the sensor value is the same (accumulated also between axes) this many times, flag it More... | |
Definition at line 47 of file data_validator.h.
|
default |
|
default |
|
privatedelete |
float DataValidator::confidence | ( | uint64_t | timestamp | ) |
Get the confidence of this validator.
Definition at line 103 of file data_validator.cpp.
References _error_count, _error_density, _error_mask, _time_last, _timeout_interval, _value_equal_count, _value_equal_count_threshold, ERROR_DENSITY_WINDOW, ERROR_FLAG_HIGH_ERRCOUNT, ERROR_FLAG_HIGH_ERRDENSITY, ERROR_FLAG_NO_DATA, ERROR_FLAG_NO_ERROR, ERROR_FLAG_STALE_DATA, ERROR_FLAG_TIMEOUT, f(), and NORETURN_ERRCOUNT.
Referenced by Sensors::diff_pres_poll(), DataValidatorGroup::get_best(), DataValidatorGroup::get_vibration_factor(), DataValidatorGroup::get_vibration_offset(), print(), setSibling(), test_error_tracking(), test_init(), test_put(), test_sensor_failure(), and test_stale_detector().
|
inline |
Get the error count of this validator.
Definition at line 92 of file data_validator.h.
References _error_count.
Referenced by test_error_tracking(), test_init(), and test_simple_failover().
|
inline |
Get the timeout value.
Definition at line 160 of file data_validator.h.
References _timeout_interval.
Referenced by add_validator_to_group(), DataValidatorGroup::DataValidatorGroup(), and test_init().
|
privatedelete |
void DataValidator::print | ( | ) |
Print the validator value.
Definition at line 148 of file data_validator.cpp.
References _lp, _mean, _rms, _time_last, _value, confidence(), dimensions, ecl_absolute_time, and ECL_INFO.
Referenced by dump_validator_state(), DataValidatorGroup::print(), Sensors::print_status(), test_init(), and vibration_offset().
|
inline |
Get the priority of this validator.
Definition at line 110 of file data_validator.h.
References _priority.
Referenced by DataValidatorGroup::get_best(), DataValidatorGroup::print(), and test_init().
void DataValidator::put | ( | uint64_t | timestamp, |
float | val, | ||
uint64_t | error_count, | ||
int | priority | ||
) |
Put an item into the validator.
val | Item to put |
Definition at line 47 of file data_validator.cpp.
References data, and dimensions.
Referenced by Sensors::diff_pres_poll(), fill_validator_with_samples(), insert_values_around_mean(), DataValidatorGroup::put(), and test_error_tracking().
void DataValidator::put | ( | uint64_t | timestamp, |
const float | val[dimensions], | ||
uint64_t | error_count, | ||
int | priority | ||
) |
Put a 3D item into the validator.
val | Item to put |
Definition at line 55 of file data_validator.cpp.
References _error_count, _error_density, _event_count, _lp, _M2, _mean, _priority, _rms, _time_last, _value, _value_equal_count, _vibe, dimensions, and f().
|
inline |
Reset the error state of this validator.
Definition at line 121 of file data_validator.h.
References _error_mask, and ERROR_FLAG_NO_ERROR.
Referenced by DataValidatorGroup::get_best(), and test_error_tracking().
|
inline |
Get the RMS values of this validator.
Definition at line 127 of file data_validator.h.
References _rms.
Referenced by DataValidatorGroup::get_vibration_factor(), test_rms_calculation(), and test_vibration().
|
inline |
Set the equal count threshold.
threshold | The number of equal values before considering the sensor stale |
Definition at line 153 of file data_validator.h.
References _value_equal_count_threshold.
Referenced by add_validator_to_group(), fill_validator_with_samples(), Sensors::Sensors(), DataValidatorGroup::set_equal_value_threshold(), test_error_tracking(), and test_rms_calculation().
|
inline |
Set the timeout value.
timeout_interval_us | The timeout interval in microseconds |
Definition at line 146 of file data_validator.h.
References _timeout_interval.
Referenced by DataValidatorGroup::add_new_validator(), fill_validator_with_samples(), Sensors::Sensors(), DataValidatorGroup::set_timeout(), test_error_tracking(), and test_init().
|
inline |
Set the sibling to the next node in the group.
Definition at line 80 of file data_validator.h.
References _sibling, and confidence().
Referenced by DataValidatorGroup::add_new_validator(), DataValidatorGroup::DataValidatorGroup(), and test_init().
|
inline |
Get the next sibling in the group.
Definition at line 74 of file data_validator.h.
References _sibling.
Referenced by DataValidatorGroup::failover_index(), DataValidatorGroup::failover_state(), DataValidatorGroup::get_best(), DataValidatorGroup::get_vibration_factor(), DataValidatorGroup::get_vibration_offset(), DataValidatorGroup::print(), DataValidatorGroup::put(), DataValidatorGroup::set_equal_value_threshold(), DataValidatorGroup::set_timeout(), test_init(), and DataValidatorGroup::~DataValidatorGroup().
|
inline |
Get the error state of this validator.
Definition at line 116 of file data_validator.h.
References _error_mask.
Referenced by dump_validator_state(), DataValidatorGroup::failover_index(), DataValidatorGroup::failover_state(), DataValidatorGroup::print(), test_error_tracking(), test_init(), test_put(), test_sensor_failure(), test_simple_failover(), and test_stale_detector().
|
inline |
Get the used status of this validator.
Definition at line 104 of file data_validator.h.
References _time_last.
Referenced by DataValidatorGroup::failover_index(), DataValidatorGroup::failover_state(), DataValidatorGroup::print(), test_error_tracking(), test_init(), and test_put().
|
inline |
Get the values of this validator.
Definition at line 98 of file data_validator.h.
References _value.
Referenced by DataValidatorGroup::get_best(), and test_put().
|
inline |
Get the vibration offset.
Definition at line 133 of file data_validator.h.
References _vibe, and print().
Referenced by DataValidatorGroup::get_vibration_offset(), test_init(), test_rms_calculation(), and test_vibration().
|
private |
error count
Definition at line 179 of file data_validator.h.
Referenced by confidence(), error_count(), and put().
|
private |
ratio between successful reads and errors
Definition at line 181 of file data_validator.h.
Referenced by confidence(), and put().
|
private |
sensor error state
Definition at line 173 of file data_validator.h.
Referenced by confidence(), reset_state(), and state().
|
private |
|
private |
|
private |
|
private |
|
private |
sensor nominal priority
Definition at line 183 of file data_validator.h.
Referenced by priority(), and put().
|
private |
root mean square error
Definition at line 188 of file data_validator.h.
|
private |
sibling in the group
Definition at line 195 of file data_validator.h.
Referenced by setSibling(), and sibling().
|
private |
last timestamp
Definition at line 177 of file data_validator.h.
Referenced by confidence(), print(), put(), and used().
|
private |
interval in which the datastream times out in us
Definition at line 175 of file data_validator.h.
Referenced by confidence(), get_timeout(), and set_timeout().
|
private |
last value
Definition at line 189 of file data_validator.h.
|
private |
equal values in a row
Definition at line 192 of file data_validator.h.
Referenced by confidence(), and put().
|
private |
when to consider an equal count as a problem
Definition at line 193 of file data_validator.h.
Referenced by confidence(), and set_equal_value_threshold().
|
private |
vibration level, in sensor unit
Definition at line 190 of file data_validator.h.
Referenced by put(), and vibration_offset().
|
static |
Definition at line 50 of file data_validator.h.
Referenced by fill_one_with_valid_data(), fill_two_with_valid_data(), print(), put(), test_priority_switch(), and test_simple_failover().
|
staticprivate |
window in measurement counts for errors
Definition at line 198 of file data_validator.h.
Referenced by confidence().
|
static |
Definition at line 169 of file data_validator.h.
Referenced by sensors::VotedSensorsUpdate::checkFailover(), confidence(), DataValidatorGroup::print(), and test_error_tracking().
|
static |
Definition at line 170 of file data_validator.h.
Referenced by sensors::VotedSensorsUpdate::checkFailover(), confidence(), DataValidatorGroup::print(), and test_error_tracking().
|
static |
Definition at line 166 of file data_validator.h.
Referenced by sensors::VotedSensorsUpdate::checkFailover(), confidence(), dump_validator_state(), DataValidatorGroup::print(), and test_init().
|
static |
Data validator error states.
Definition at line 165 of file data_validator.h.
Referenced by sensors::VotedSensorsUpdate::checkFailover(), confidence(), DataValidatorGroup::failover_index(), DataValidatorGroup::failover_state(), DataValidatorGroup::print(), reset_state(), setup_base_group(), and test_simple_failover().
|
static |
Definition at line 167 of file data_validator.h.
Referenced by sensors::VotedSensorsUpdate::checkFailover(), confidence(), dump_validator_state(), DataValidatorGroup::print(), and test_stale_detector().
|
static |
Definition at line 168 of file data_validator.h.
Referenced by sensors::VotedSensorsUpdate::checkFailover(), confidence(), dump_validator_state(), DataValidatorGroup::print(), test_put(), and test_sensor_failure().
|
staticprivate |
if the error count reaches this value, return sensor as invalid
Definition at line 197 of file data_validator.h.
Referenced by confidence().
|
staticprivate |
if the sensor value is the same (accumulated also between axes) this many times, flag it
Definition at line 199 of file data_validator.h.