From f20726d47f0fc19eff0782e8385644daba9ded48 Mon Sep 17 00:00:00 2001 From: kamilritz Date: Thu, 23 Jan 2020 14:41:58 +0100 Subject: [PATCH] Expand auto-format coverage and tiny style changes --- .clang-format | 6 +- EKF/RingBuffer.h | 2 + EKF/imu_down_sampler.cpp | 26 +++------ EKF/imu_down_sampler.hpp | 9 +-- EKF/vel_pos_fusion.cpp | 81 ++++++++++++-------------- mathlib/mathlib.cpp | 30 +++------- mathlib/mathlib.h | 9 ++- tools/format.sh | 6 ++ validation/data_validator.cpp | 27 +++------ validation/data_validator.h | 90 +++++++++++++++-------------- validation/data_validator_group.cpp | 82 +++++++++++--------------- validation/data_validator_group.h | 38 ++++++------ 12 files changed, 178 insertions(+), 228 deletions(-) diff --git a/.clang-format b/.clang-format index d72733e614..6edbfee64f 100644 --- a/.clang-format +++ b/.clang-format @@ -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 diff --git a/EKF/RingBuffer.h b/EKF/RingBuffer.h index db3a94539f..8f2f7312f2 100644 --- a/EKF/RingBuffer.h +++ b/EKF/RingBuffer.h @@ -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) { diff --git a/EKF/imu_down_sampler.cpp b/EKF/imu_down_sampler.cpp index 31880d72c3..1c39c7fc4e 100644 --- a/EKF/imu_down_sampler.cpp +++ b/EKF/imu_down_sampler.cpp @@ -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; diff --git a/EKF/imu_down_sampler.hpp b/EKF/imu_down_sampler.hpp index 21583dc4a6..3409cf1309 100644 --- a/EKF/imu_down_sampler.hpp +++ b/EKF/imu_down_sampler.hpp @@ -37,15 +37,14 @@ */ #pragma once -#include #include +#include #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(); - }; - diff --git a/EKF/vel_pos_fusion.cpp b/EKF/vel_pos_fusion.cpp index b96cf72d8f..d1dc525179 100644 --- a/EKF/vel_pos_fusion.cpp +++ b/EKF/vel_pos_fusion.cpp @@ -41,39 +41,37 @@ * */ -#include "ekf.h" #include #include +#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 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; diff --git a/mathlib/mathlib.cpp b/mathlib/mathlib.cpp index 69aee15ffc..1281af0ad7 100644 --- a/mathlib/mathlib.cpp +++ b/mathlib/mathlib.cpp @@ -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 */ diff --git a/mathlib/mathlib.h b/mathlib/mathlib.h index 04a1803257..42de1c6b4a 100644 --- a/mathlib/mathlib.h +++ b/mathlib/mathlib.h @@ -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 -#endif //ECL_STANDALONE -#endif //MATHLIB_H +#endif // ECL_STANDALONE +#endif // MATHLIB_H diff --git a/tools/format.sh b/tools/format.sh index 5f5bab9966..3a33c30df9 100755 --- a/tools/format.sh +++ b/tools/format.sh @@ -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' diff --git a/validation/data_validator.cpp b/validation/data_validator.cpp index 4b2f8ce7c6..6ed6cbb802 100644 --- a/validation/data_validator.cpp +++ b/validation/data_validator.cpp @@ -43,17 +43,13 @@ #include -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())); } } diff --git a/validation/data_validator.h b/validation/data_validator.h index 6bda5c3e93..7bd4978142 100644 --- a/validation/data_validator.h +++ b/validation/data_validator.h @@ -44,8 +44,7 @@ #include #include -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; diff --git a/validation/data_validator_group.cpp b/validation/data_validator_group.cpp index 719275a415..962fb3043e 100644 --- a/validation/data_validator_group.cpp +++ b/validation/data_validator_group.cpp @@ -43,8 +43,8 @@ #include #include -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; diff --git a/validation/data_validator_group.h b/validation/data_validator_group.h index 4fd6ab0a1c..89c6cfd38f 100644 --- a/validation/data_validator_group.h +++ b/validation/data_validator_group.h @@ -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;