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

Expand auto-format coverage and tiny style changes

This commit is contained in:
kamilritz 2020-01-23 14:41:58 +01:00 committed by Mathieu Bresciani
parent fbdd75da2e
commit f20726d47f
12 changed files with 178 additions and 228 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
.clang-format
View File

@ -76,7 +76,7 @@ IndentWidth: 8 # Modified
IndentWrappedFunctionNames: false
JavaScriptQuotes: Leave
JavaScriptWrapImports: true
KeepEmptyLinesAtTheStartOfBlocks: false
KeepEmptyLinesAtTheStartOfBlocks: true # Modified
MacroBlockBegin: ''
MacroBlockEnd: ''
MaxEmptyLinesToKeep: 1
@ -84,8 +84,8 @@ NamespaceIndentation: None
ObjCBlockIndentWidth: 2
ObjCSpaceAfterProperty: false
ObjCSpaceBeforeProtocolList: false
PenaltyBreakAssignment: 2
PenaltyBreakBeforeFirstCallParameter: 1
PenaltyBreakAssignment: 200 # Modified
PenaltyBreakBeforeFirstCallParameter: 20 # Modified
PenaltyBreakComment: 300
PenaltyBreakFirstLessLess: 120
PenaltyBreakString: 1000

2
EKF/RingBuffer.h
View File

@ -60,6 +60,7 @@ public:
RingBuffer &operator=(RingBuffer &&) = delete;
bool allocate(uint8_t size) {
if (_buffer != nullptr) {
delete[] _buffer;
}
@ -92,6 +93,7 @@ public:
}
void push(const data_type &sample) {
uint8_t head_new = _head;
if (!_first_write) {

26
EKF/imu_down_sampler.cpp
View File

@ -1,24 +1,18 @@
#include "imu_down_sampler.hpp"
ImuDownSampler::ImuDownSampler(float target_dt_sec):
_target_dt{target_dt_sec},
_imu_collection_time_adj{0.0f}
{
ImuDownSampler::ImuDownSampler(float target_dt_sec) : _target_dt{target_dt_sec}, _imu_collection_time_adj{0.0f} {
reset();
_imu_down_sampled.time_us = 0.0f;
}
ImuDownSampler::~ImuDownSampler()
{
}
ImuDownSampler::~ImuDownSampler() {}
// integrate imu samples until target dt reached
// assumes that dt of the gyroscope is close to the dt of the accelerometer
// returns true if target dt is reached
bool ImuDownSampler::update(imuSample imu_sample_new)
{
if(_do_reset)
{
bool ImuDownSampler::update(imuSample imu_sample_new) {
if (_do_reset) {
reset();
}
// accumulate time deltas
@ -42,11 +36,11 @@ bool ImuDownSampler::update(imuSample imu_sample_new)
// check if the target time delta between filter prediction steps has been exceeded
if (_imu_down_sampled.delta_ang_dt >= _target_dt - _imu_collection_time_adj) {
// accumulate the amount of time to advance the IMU collection time so that we meet the
// average EKF update rate requirement
_imu_collection_time_adj += 0.01f * (_imu_down_sampled.delta_ang_dt - _target_dt);
_imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * _target_dt, 0.5f * _target_dt);
_imu_collection_time_adj = math::constrain(_imu_collection_time_adj, -0.5f * _target_dt,
0.5f * _target_dt);
_imu_down_sampled.delta_ang = _delta_angle_accumulated.to_axis_angle();
@ -57,14 +51,12 @@ bool ImuDownSampler::update(imuSample imu_sample_new)
}
}
imuSample ImuDownSampler::getDownSampledImuAndTriggerReset()
{
imuSample ImuDownSampler::getDownSampledImuAndTriggerReset() {
_do_reset = true;
return _imu_down_sampled;
}
void ImuDownSampler::reset()
{
void ImuDownSampler::reset() {
_imu_down_sampled.delta_ang.setZero();
_imu_down_sampled.delta_vel.setZero();
_imu_down_sampled.delta_ang_dt = 0.0f;

9
EKF/imu_down_sampler.hpp
View File

@ -37,15 +37,14 @@
*/
#pragma once
#include <matrix/math.hpp>
#include <mathlib/mathlib.h>
#include <matrix/math.hpp>
#include "common.h"
using namespace estimator;
class ImuDownSampler
{
class ImuDownSampler {
public:
ImuDownSampler(float target_dt_sec);
~ImuDownSampler();
@ -56,11 +55,9 @@ public:
private:
imuSample _imu_down_sampled;
Quatf _delta_angle_accumulated;
const float _target_dt; // [sec]
const float _target_dt; // [sec]
float _imu_collection_time_adj;
bool _do_reset;
void reset();
};

81
EKF/vel_pos_fusion.cpp
View File

@ -41,39 +41,37 @@
*
*/
#include "ekf.h"
#include <ecl.h>
#include <mathlib/mathlib.h>
#include "ekf.h"
bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) {
bool Ekf::fuseHorizontalVelocity(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(0) = P(4,4) + obs_var(0);
innov_var(1) = P(5,5) + obs_var(1);
innov_var(0) = P(4, 4) + obs_var(0);
innov_var(1) = P(5, 5) + obs_var(1);
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
const bool innov_check_pass = (test_ratio(0) <= 1.0f);
if (innov_check_pass)
{
if (innov_check_pass) {
_time_last_hor_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_hor_vel = false;
fuseVelPosHeight(innov(0),innov_var(0),0);
fuseVelPosHeight(innov(1),innov_var(1),1);
fuseVelPosHeight(innov(0), innov_var(0), 0);
fuseVelPosHeight(innov(1), innov_var(1), 1);
return true;
}else{
} else {
_innov_check_fail_status.flags.reject_hor_vel = true;
return false;
}
}
bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(2) = P(6,6) + obs_var(2);
bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) {
innov_var(2) = P(6, 6) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
const bool innov_check_pass = (test_ratio(1) <= 1.0f);
@ -81,20 +79,20 @@ bool Ekf::fuseVerticalVelocity(const Vector3f &innov, const Vector2f &innov_gate
_time_last_ver_vel_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_vel = false;
fuseVelPosHeight(innov(2),innov_var(2),2);
fuseVelPosHeight(innov(2), innov_var(2), 2);
return true;
}else{
} else {
_innov_check_fail_status.flags.reject_ver_vel = true;
return false;
}
}
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(0) = P(7,7) + obs_var(0);
innov_var(1) = P(8,8) + obs_var(1);
bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) {
innov_var(0) = P(7, 7) + obs_var(0);
innov_var(1) = P(8, 8) + obs_var(1);
test_ratio(0) = fmaxf(sq(innov(0)) / (sq(innov_gate(0)) * innov_var(0)),
sq(innov(1)) / (sq(innov_gate(0)) * innov_var(1)));
@ -108,20 +106,20 @@ bool Ekf::fuseHorizontalPosition(const Vector3f &innov, const Vector2f &innov_ga
}
_innov_check_fail_status.flags.reject_hor_pos = false;
fuseVelPosHeight(innov(0),innov_var(0),3);
fuseVelPosHeight(innov(1),innov_var(1),4);
fuseVelPosHeight(innov(0), innov_var(0), 3);
fuseVelPosHeight(innov(1), innov_var(1), 4);
return true;
}else{
} else {
_innov_check_fail_status.flags.reject_hor_pos = true;
return false;
}
}
bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate,
const Vector3f &obs_var, Vector3f &innov_var, Vector2f &test_ratio)
{
innov_var(2) = P(9,9) + obs_var(2);
bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate, const Vector3f &obs_var,
Vector3f &innov_var, Vector2f &test_ratio) {
innov_var(2) = P(9, 9) + obs_var(2);
test_ratio(1) = sq(innov(2)) / (sq(innov_gate(1)) * innov_var(2));
const bool innov_check_pass = (test_ratio(1) <= 1.0f) || !_control_status.flags.tilt_align;
@ -129,31 +127,31 @@ bool Ekf::fuseVerticalPosition(const Vector3f &innov, const Vector2f &innov_gate
_time_last_hgt_fuse = _time_last_imu;
_innov_check_fail_status.flags.reject_ver_pos = false;
fuseVelPosHeight(innov(2),innov_var(2),5);
fuseVelPosHeight(innov(2), innov_var(2), 5);
return true;
}else{
} else {
_innov_check_fail_status.flags.reject_ver_pos = true;
return false;
}
}
// Helper function that fuses a single velocity or position measurement
void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index)
{
float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used.
const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int obs_index) {
float Kfusion[24] = {}; // Kalman gain vector for any single observation - sequential fusion is used.
const unsigned state_index = obs_index + 4; // we start with vx and this is the 4. state
// calculate kalman gain K = PHS, where S = 1/innovation variance
for (int row = 0; row < _k_num_states; row++) {
Kfusion[row] = P(row,state_index) / innov_var;
Kfusion[row] = P(row, state_index) / innov_var;
}
matrix::SquareMatrix<float, _k_num_states> KHP;
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
KHP(row,column) = Kfusion[row] * P(state_index,column);
KHP(row, column) = Kfusion[row] * P(state_index, column);
}
}
@ -162,7 +160,7 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
bool healthy = true;
for (int i = 0; i < _k_num_states; i++) {
if (P(i,i) < KHP(i,i)) {
if (P(i, i) < KHP(i, i)) {
// zero rows and columns
P.uncorrelateCovarianceSetVariance<1>(i, 0.0f);
@ -175,13 +173,12 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
}
}
// only apply covariance and state corrections if healthy
if (healthy) {
// apply the covariance corrections
for (unsigned row = 0; row < _k_num_states; row++) {
for (unsigned column = 0; column < _k_num_states; column++) {
P(row,column) = P(row,column) - KHP(row,column);
P(row, column) = P(row, column) - KHP(row, column);
}
}
@ -190,12 +187,10 @@ void Ekf::fuseVelPosHeight(const float innov, const float innov_var, const int o
// apply the state corrections
fuse(Kfusion, innov);
}
}
void Ekf::setVelPosFaultStatus(const int index, const bool status)
{
void Ekf::setVelPosFaultStatus(const int index, const bool status) {
if (index == 0) {
_fault_status.flags.bad_vel_N = status;

30
mathlib/mathlib.cpp
View File

@ -42,34 +42,18 @@
#ifdef ECL_STANDALONE
namespace math
{
namespace math {
float min(float val1, float val2)
{
return (val1 < val2) ? val1 : val2;
}
float min(float val1, float val2) { return (val1 < val2) ? val1 : val2; }
float max(float val1, float val2)
{
return (val1 > val2) ? val1 : val2;
}
float max(float val1, float val2) { return (val1 > val2) ? val1 : val2; }
float constrain(float val, float min, float max)
{
return (val < min) ? min : ((val > max) ? max : val);
}
float constrain(float val, float min, float max) { return (val < min) ? min : ((val > max) ? max : val); }
float radians(float degrees)
{
return (degrees / 180.0f) * M_PI_F;
}
float radians(float degrees) { return (degrees / 180.0f) * M_PI_F; }
float degrees(float radians)
{
return (radians * 180.0f) / M_PI_F;
}
float degrees(float radians) { return (radians * 180.0f) / M_PI_F; }
} // namespace math
} // namespace math
#endif /* ECL_STANDALONE */

9
mathlib/mathlib.h
View File

@ -51,8 +51,7 @@
#define M_PI_2_F (M_PI / 2.0f)
#endif
namespace math
{
namespace math {
// using namespace Eigen;
float min(float val1, float val2);
@ -61,10 +60,10 @@ float constrain(float val, float min, float max);
float radians(float degrees);
float degrees(float radians);
}
} // namespace math
#else
#include <mathlib/mathlib.h>
#endif //ECL_STANDALONE
#endif //MATHLIB_H
#endif // ECL_STANDALONE
#endif // MATHLIB_H

6
tools/format.sh
View File

@ -3,6 +3,12 @@ do_format=$1
files_to_format="""
EKF/AlphaFilter.hpp
EKF/RingBuffer.h
EKF/vel_pos_fusion.cpp
EKF/imu_down_sampler.*pp
mathlib/*.cpp
mathlib/*.h
validation/*.cpp
validation/*.h
"""
RED='\033[0;31m'
GREEN='\033[0;32m'

27
validation/data_validator.cpp
View File

@ -43,17 +43,13 @@
#include <ecl.h>
void
DataValidator::put(uint64_t timestamp, float val, uint64_t error_count_in, int priority_in)
{
float data[dimensions] = { val }; //sets the first value and all others to 0
void DataValidator::put(uint64_t timestamp, float val, uint64_t error_count_in, int priority_in) {
float data[dimensions] = {val}; // sets the first value and all others to 0
put(timestamp, data, error_count_in, priority_in);
}
void
DataValidator::put(uint64_t timestamp, const float val[dimensions], uint64_t error_count_in, int priority_in)
{
void DataValidator::put(uint64_t timestamp, const float val[dimensions], uint64_t error_count_in, int priority_in) {
_event_count++;
if (error_count_in > _error_count) {
@ -99,9 +95,8 @@ DataValidator::put(uint64_t timestamp, const float val[dimensions], uint64_t err
_time_last = timestamp;
}
float
DataValidator::confidence(uint64_t timestamp)
{
float DataValidator::confidence(uint64_t timestamp) {
float ret = 1.0f;
/* check if we have any data */
@ -128,7 +123,6 @@ DataValidator::confidence(uint64_t timestamp)
/* cap error density counter at window size */
_error_mask |= ERROR_FLAG_HIGH_ERRDENSITY;
_error_density = ERROR_DENSITY_WINDOW;
}
/* no critical errors */
@ -144,17 +138,14 @@ DataValidator::confidence(uint64_t timestamp)
return ret;
}
void
DataValidator::print()
{
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(ecl_absolute_time()));
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(ecl_absolute_time()));
}
}

90
validation/data_validator.h
View File

@ -44,8 +44,7 @@
#include <math.h>
#include <stdint.h>
class DataValidator
{
class DataValidator {
public:
static const unsigned dimensions = 3;
@ -57,146 +56,149 @@ public:
*
* @param val Item to put
*/
void put(uint64_t timestamp, float val, uint64_t error_count, int priority);
void put(uint64_t timestamp, float val, uint64_t error_count, int priority);
/**
* Put a 3D item into the validator.
*
* @param val Item to put
*/
void put(uint64_t timestamp, const float val[dimensions], uint64_t error_count, int priority);
void put(uint64_t timestamp, const float val[dimensions], uint64_t error_count, int priority);
/**
* Get the next sibling in the group
*
* @return the next sibling
*/
DataValidator *sibling() { return _sibling; }
DataValidator *sibling() { return _sibling; }
/**
* Set the sibling to the next node in the group
*
*/
void setSibling(DataValidator *new_sibling) { _sibling = new_sibling; }
void setSibling(DataValidator *new_sibling) { _sibling = new_sibling; }
/**
* Get the confidence of this validator
* @return the confidence between 0 and 1
*/
float confidence(uint64_t timestamp);
float confidence(uint64_t timestamp);
/**
* Get the error count of this validator
* @return the error count
*/
uint64_t error_count() const { return _error_count; }
uint64_t error_count() const { return _error_count; }
/**
* Get the values of this validator
* @return the stored value
*/
float *value() { return _value; }
float *value() { return _value; }
/**
* Get the used status of this validator
* @return true if this validator ever saw data
*/
bool used() const { return (_time_last > 0); }
bool used() const { return (_time_last > 0); }
/**
* Get the priority of this validator
* @return the stored priority
*/
int priority() const { return _priority; }
int priority() const { return _priority; }
/**
* Get the error state of this validator
* @return the bitmask with the error status
*/
uint32_t state() const { return _error_mask; }
uint32_t state() const { return _error_mask; }
/**
* Reset the error state of this validator
*/
void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; }
void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; }
/**
* Get the RMS values of this validator
* @return the stored RMS
*/
float *rms() { return _rms; }
float *rms() { return _rms; }
/**
* Get the vibration offset
* @return the stored vibration offset
*/
float *vibration_offset() { return _vibe; }
float *vibration_offset() { return _vibe; }
/**
* Print the validator value
*
*/
void print();
void print();
/**
* Set the timeout value
*
* @param timeout_interval_us The timeout interval in microseconds
*/
void set_timeout(uint32_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
void set_timeout(uint32_t timeout_interval_us) { _timeout_interval = timeout_interval_us; }
/**
* Set the equal count threshold
*
* @param threshold The number of equal values before considering the sensor stale
*/
void set_equal_value_threshold(uint32_t threshold) { _value_equal_count_threshold = threshold; }
void set_equal_value_threshold(uint32_t threshold) { _value_equal_count_threshold = threshold; }
/**
* Get the timeout value
*
* @return The timeout interval in microseconds
*/
uint32_t get_timeout() const { return _timeout_interval; }
uint32_t get_timeout() const { return _timeout_interval; }
/**
* 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);
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:
uint32_t _error_mask{ERROR_FLAG_NO_ERROR}; /**< sensor error state */
uint32_t _error_mask{ERROR_FLAG_NO_ERROR}; /**< sensor error state */
uint32_t _timeout_interval{20000}; /**< interval in which the datastream times out in us */
uint32_t _timeout_interval{20000}; /**< interval in which the datastream times out in us */
uint64_t _time_last{0}; /**< last timestamp */
uint64_t _event_count{0}; /**< total data counter */
uint64_t _error_count{0}; /**< error count */
uint64_t _time_last{0}; /**< last timestamp */
uint64_t _event_count{0}; /**< total data counter */
uint64_t _error_count{0}; /**< error count */
int _error_density{0}; /**< ratio between successful reads and errors */
int _error_density{0}; /**< ratio between successful reads and errors */
int _priority{0}; /**< sensor nominal priority */
int _priority{0}; /**< sensor nominal priority */
float _mean[dimensions] {}; /**< mean of value */
float _lp[dimensions] {}; /**< low pass value */
float _M2[dimensions] {}; /**< RMS component value */
float _rms[dimensions] {}; /**< root mean square error */
float _value[dimensions] {}; /**< last value */
float _vibe[dimensions] {}; /**< vibration level, in sensor unit */
float _mean[dimensions]{}; /**< mean of value */
float _lp[dimensions]{}; /**< low pass value */
float _M2[dimensions]{}; /**< RMS component value */
float _rms[dimensions]{}; /**< root mean square error */
float _value[dimensions]{}; /**< last value */
float _vibe[dimensions]{}; /**< vibration level, in sensor unit */
unsigned _value_equal_count{0}; /**< equal values in a row */
unsigned _value_equal_count_threshold{VALUE_EQUAL_COUNT_DEFAULT}; /**< when to consider an equal count as a problem */
unsigned _value_equal_count{0}; /**< equal values in a row */
unsigned _value_equal_count_threshold{
VALUE_EQUAL_COUNT_DEFAULT}; /**< when to consider an equal count as a problem */
DataValidator *_sibling{nullptr}; /**< sibling in the group */
DataValidator *_sibling{nullptr}; /**< sibling in the group */
static const constexpr unsigned NORETURN_ERRCOUNT = 10000; /**< if the error count reaches this value, return sensor as invalid */
static const constexpr float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */
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 */
static const constexpr unsigned NORETURN_ERRCOUNT =
10000; /**< if the error count reaches this value, return sensor as invalid */
static const constexpr float ERROR_DENSITY_WINDOW = 100.0f; /**< window in measurement counts for errors */
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 */
/* we don't want this class to be copied */
DataValidator(const DataValidator &) = delete;

82
validation/data_validator_group.cpp
View File

@ -43,8 +43,8 @@
#include <ecl.h>
#include <float.h>
DataValidatorGroup::DataValidatorGroup(unsigned siblings)
{
DataValidatorGroup::DataValidatorGroup(unsigned siblings) {
DataValidator *next = nullptr;
DataValidator *prev = nullptr;
@ -68,8 +68,7 @@ DataValidatorGroup::DataValidatorGroup(unsigned siblings)
}
}
DataValidatorGroup::~DataValidatorGroup()
{
DataValidatorGroup::~DataValidatorGroup() {
while (_first) {
DataValidator *next = _first->sibling();
delete (_first);
@ -77,8 +76,8 @@ DataValidatorGroup::~DataValidatorGroup()
}
}
DataValidator *DataValidatorGroup::add_new_validator()
{
DataValidator *DataValidatorGroup::add_new_validator() {
DataValidator *validator = new DataValidator();
if (!validator) {
@ -91,9 +90,8 @@ DataValidator *DataValidatorGroup::add_new_validator()
return _last;
}
void
DataValidatorGroup::set_timeout(uint32_t timeout_interval_us)
{
void DataValidatorGroup::set_timeout(uint32_t timeout_interval_us) {
DataValidator *next = _first;
while (next != nullptr) {
@ -104,9 +102,8 @@ DataValidatorGroup::set_timeout(uint32_t timeout_interval_us)
_timeout_interval_us = timeout_interval_us;
}
void
DataValidatorGroup::set_equal_value_threshold(uint32_t threshold)
{
void DataValidatorGroup::set_equal_value_threshold(uint32_t threshold) {
DataValidator *next = _first;
while (next != nullptr) {
@ -115,10 +112,9 @@ DataValidatorGroup::set_equal_value_threshold(uint32_t threshold)
}
}
void DataValidatorGroup::put(unsigned index, uint64_t timestamp, const float val[3], uint64_t error_count,
int priority) {
void
DataValidatorGroup::put(unsigned index, uint64_t timestamp, const float val[3], uint64_t error_count, int priority)
{
DataValidator *next = _first;
unsigned i = 0;
@ -133,9 +129,8 @@ DataValidatorGroup::put(unsigned index, uint64_t timestamp, const float val[3],
}
}
float *
DataValidatorGroup::get_best(uint64_t timestamp, int *index)
{
float *DataValidatorGroup::get_best(uint64_t timestamp, int *index) {
DataValidator *next = _first;
// XXX This should eventually also include voting
@ -164,9 +159,8 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
*/
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))
) && (confidence > 0.0f)) {
(fabsf(confidence - max_confidence) < 0.01f && (next->priority() > max_priority))) &&
(confidence > 0.0f)) {
max_index = i;
max_confidence = confidence;
max_priority = next->priority();
@ -180,13 +174,11 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
/* the current best sensor is not matching the previous best sensor,
* or the only sensor went bad */
if (max_index != _curr_best || ((max_confidence < FLT_EPSILON) && (_curr_best >= 0))) {
bool true_failsafe = true;
/* 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;
@ -222,16 +214,14 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
return (best) ? best->value() : nullptr;
}
float
DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
{
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();
@ -248,16 +238,14 @@ DataValidatorGroup::get_vibration_factor(uint64_t timestamp)
return vibe;
}
float
DataValidatorGroup::get_vibration_offset(uint64_t timestamp, int axis)
{
float DataValidatorGroup::get_vibration_offset(uint64_t timestamp, int axis) {
DataValidator *next = _first;
float vibe = -1.0f;
/* find the best vibration value of a non-timed out sensor */
while (next != nullptr) {
if (next->confidence(timestamp) > 0.5f) {
float *vibration_offset = next->vibration_offset();
@ -272,13 +260,10 @@ DataValidatorGroup::get_vibration_offset(uint64_t timestamp, int axis)
return vibe;
}
void
DataValidatorGroup::print()
{
/* print the group's state */
ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)",
_curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO",
_toggle_count);
void DataValidatorGroup::print() {
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;
@ -303,14 +288,14 @@ DataValidatorGroup::print()
}
}
int
DataValidatorGroup::failover_index()
{
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)) {
if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) &&
(i == (unsigned)_prev_best)) {
return i;
}
@ -321,14 +306,14 @@ DataValidatorGroup::failover_index()
return -1;
}
uint32_t
DataValidatorGroup::failover_state()
{
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)) {
if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) &&
(i == (unsigned)_prev_best)) {
return next->state();
}
@ -339,9 +324,8 @@ DataValidatorGroup::failover_state()
return DataValidator::ERROR_FLAG_NO_ERROR;
}
uint32_t
DataValidatorGroup::get_sensor_state(unsigned index)
{
uint32_t DataValidatorGroup::get_sensor_state(unsigned index) {
DataValidator *next = _first;
unsigned i = 0;

38
validation/data_validator_group.h
View File

@ -43,8 +43,7 @@
#include "data_validator.h"
class DataValidatorGroup
{
class DataValidatorGroup {
public:
/**
* @param siblings initial number of DataValidator's. Must be > 0.
@ -67,90 +66,89 @@ public:
* @param error_count The current error count of the sensor
* @param priority The priority of the sensor
*/
void put(unsigned index, uint64_t timestamp, const float val[3], uint64_t error_count, int priority);
void put(unsigned index, uint64_t timestamp, const 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);
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);
float get_vibration_factor(uint64_t timestamp);
/**
* Get the vibration offset in the sensor unit
*
* @return float value representing the vibration offset
*/
float get_vibration_offset(uint64_t timestamp, int axis);
float get_vibration_offset(uint64_t timestamp, int axis);
/**
* Get the number of failover events
*
* @return the number of failovers
*/
unsigned failover_count() const { return _toggle_count; }
unsigned failover_count() const { return _toggle_count; }
/**
* Get the index of the failed sensor in the group
*
* @return index of the failed sensor
*/
int failover_index();
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();
uint32_t failover_state();
/**
* Get the error state of the sensor with the specified index
*
* @return bitmask with error states of the sensor
*/
uint32_t get_sensor_state(unsigned index);
uint32_t get_sensor_state(unsigned index);
/**
* Print the validator value
*
*/
void print();
void print();
/**
* Set the timeout value for the whole group
*
* @param timeout_interval_us The timeout interval in microseconds
*/
void set_timeout(uint32_t timeout_interval_us);
void set_timeout(uint32_t timeout_interval_us);
/**
* Set the equal count threshold for the whole group
*
* @param threshold The number of equal values before considering the sensor stale
*/
void set_equal_value_threshold(uint32_t threshold);
void set_equal_value_threshold(uint32_t threshold);
private:
DataValidator *_first{nullptr}; /**< first node in the group */
DataValidator *_last{nullptr}; /**< last node in the group */
DataValidator *_first{nullptr}; /**< first node in the group */
DataValidator *_last{nullptr}; /**< last node in the group */
uint32_t _timeout_interval_us{0}; /**< currently set timeout */
int _curr_best{-1}; /**< currently best index */
int _prev_best{-1}; /**< the previous best index */
int _curr_best{-1}; /**< currently best index */
int _prev_best{-1}; /**< the previous best index */
uint64_t _first_failover_time{0}; /**< timestamp where the first failover occured or zero if none occured */
uint64_t _first_failover_time{0}; /**< timestamp where the first failover occured or zero if none occured */
unsigned _toggle_count{0}; /**< number of back and forth switches between two sensors */
unsigned _toggle_count{0}; /**< number of back and forth switches between two sensors */
static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f;