From c3e071e052b03825b3285d199abf0accc2ea8732 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 13 Jun 2018 15:48:41 -0400 Subject: [PATCH 1/3] validation initialize all members - fixes Coverity CID 260331 --- validation/data_validator.cpp | 24 +----------- validation/data_validator.h | 57 ++++++++++++++++------------- validation/data_validator_group.cpp | 16 +------- validation/data_validator_group.h | 25 ++++++++----- 4 files changed, 50 insertions(+), 72 deletions(-) diff --git a/validation/data_validator.cpp b/validation/data_validator.cpp index c8beaf6384..4b2f8ce7c6 100644 --- a/validation/data_validator.cpp +++ b/validation/data_validator.cpp @@ -40,28 +40,8 @@ */ #include "data_validator.h" -#include - -DataValidator::DataValidator() : - _error_mask(ERROR_FLAG_NO_ERROR), - _timeout_interval(20000), - _time_last(0), - _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}, - _vibe{0.0f}, - _value_equal_count(0), - _value_equal_count_threshold(VALUE_EQUAL_COUNT_DEFAULT), - _sibling(nullptr) -{ -} +#include void DataValidator::put(uint64_t timestamp, float val, uint64_t error_count_in, int priority_in) @@ -72,7 +52,7 @@ DataValidator::put(uint64_t timestamp, float val, uint64_t error_count_in, int p } void -DataValidator::put(uint64_t timestamp, float val[dimensions], uint64_t error_count_in, int priority_in) +DataValidator::put(uint64_t timestamp, const float val[dimensions], uint64_t error_count_in, int priority_in) { _event_count++; diff --git a/validation/data_validator.h b/validation/data_validator.h index ca380f86b4..c60ac21122 100644 --- a/validation/data_validator.h +++ b/validation/data_validator.h @@ -49,8 +49,8 @@ class DataValidator public: static const unsigned dimensions = 3; - DataValidator(); - virtual ~DataValidator() = default; + DataValidator() = default; + ~DataValidator() = default; /** * Put an item into the validator. @@ -64,7 +64,7 @@ public: * * @param val Item to put */ - void put(uint64_t timestamp, 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 @@ -89,7 +89,7 @@ public: * Get the error count of this validator * @return the error count */ - uint64_t error_count() { return _error_count; } + uint64_t error_count() const { return _error_count; } /** * Get the values of this validator @@ -101,19 +101,19 @@ public: * Get the used status of this validator * @return true if this validator ever saw data */ - bool used() { return (_time_last > 0); } + bool used() const { return (_time_last > 0); } /** * Get the priority of this validator * @return the stored priority */ - int priority() { return (_priority); } + int priority() const { return _priority; } /** * Get the error state of this validator * @return the bitmask with the error status */ - uint32_t state() { return _error_mask; } + uint32_t state() const { return _error_mask; } /** * Reset the error state of this validator @@ -157,7 +157,7 @@ public: * * @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 @@ -170,24 +170,29 @@ public: static constexpr uint32_t ERROR_FLAG_HIGH_ERRDENSITY = (0x00000001U << 4); private: - uint32_t _error_mask; /**< sensor error state */ - uint32_t _timeout_interval; /**< interval in which the datastream times out in us */ - uint64_t _time_last; /**< last timestamp */ - 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[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; /**< equal values in a row */ - unsigned _value_equal_count_threshold; /**< when to consider an equal count as a problem */ - - DataValidator *_sibling; /**< sibling in the group */ + 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 */ + + 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 _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 */ + + 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 */ 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 */ diff --git a/validation/data_validator_group.cpp b/validation/data_validator_group.cpp index 0df70485f3..f3419c8d24 100644 --- a/validation/data_validator_group.cpp +++ b/validation/data_validator_group.cpp @@ -43,13 +43,7 @@ #include #include -DataValidatorGroup::DataValidatorGroup(unsigned siblings) : - _first(nullptr), - _last(nullptr), - _curr_best(-1), - _prev_best(-1), - _first_failover_time(0), - _toggle_count(0) +DataValidatorGroup::DataValidatorGroup(unsigned siblings) { DataValidator *next = nullptr; DataValidator *prev = nullptr; @@ -123,7 +117,7 @@ DataValidatorGroup::set_equal_value_threshold(uint32_t threshold) void -DataValidatorGroup::put(unsigned index, uint64_t timestamp, float val[3], uint64_t error_count, int priority) +DataValidatorGroup::put(unsigned index, uint64_t timestamp, const float val[3], uint64_t error_count, int priority) { DataValidator *next = _first; unsigned i = 0; @@ -305,12 +299,6 @@ DataValidatorGroup::print() } } -unsigned -DataValidatorGroup::failover_count() -{ - return _toggle_count; -} - int DataValidatorGroup::failover_index() { diff --git a/validation/data_validator_group.h b/validation/data_validator_group.h index 34d54d4654..094d062d28 100644 --- a/validation/data_validator_group.h +++ b/validation/data_validator_group.h @@ -50,7 +50,7 @@ public: * @param siblings initial number of DataValidator's. Must be > 0. */ DataValidatorGroup(unsigned siblings); - virtual ~DataValidatorGroup(); + ~DataValidatorGroup(); /** * Create a new Validator (with index equal to the number of currently existing validators) @@ -67,7 +67,7 @@ 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, 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 @@ -95,7 +95,7 @@ public: * * @return the number of failovers */ - unsigned failover_count(); + unsigned failover_count() const { return _toggle_count; } /** * Get the index of the failed sensor in the group @@ -133,13 +133,18 @@ public: private: - DataValidator *_first; /**< first node in the group */ - DataValidator *_last; /**< last node in the group */ - uint32_t _timeout_interval_us; /**< currently set timeout */ - 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 */ + 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 */ + + 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 */ + static constexpr float MIN_REGULAR_CONFIDENCE = 0.9f; /* we don't want this class to be copied */ From 4ae8242c350a431824b342ebc63a8bd474c90653 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 13 Jun 2018 15:51:59 -0400 Subject: [PATCH 2/3] validation DataValidatorGroup::get_best() nullptr check - fixes Coverity CID 139448 --- validation/data_validator_group.cpp | 10 +++++++--- 1 file changed, 7 insertions(+), 3 deletions(-) diff --git a/validation/data_validator_group.cpp b/validation/data_validator_group.cpp index f3419c8d24..135c66bd84 100644 --- a/validation/data_validator_group.cpp +++ b/validation/data_validator_group.cpp @@ -147,12 +147,12 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) int max_index = -1; DataValidator *best = nullptr; - unsigned i = 0; + int i = 0; while (next != nullptr) { float confidence = next->confidence(timestamp); - if (static_cast(i) == pre_check_best) { + if (i == pre_check_best) { pre_check_prio = next->priority(); pre_check_confidence = confidence; } @@ -186,10 +186,14 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) /* check whether the switch was a failsafe or preferring a higher priority sensor */ if (pre_check_prio != -1 && pre_check_prio < max_priority && fabsf(pre_check_confidence - max_confidence) < 0.1f) { + /* this is not a failover */ true_failsafe = false; + /* reset error flags, this is likely a hotplug sensor coming online late */ - best->reset_state(); + if (best != nullptr) { + best->reset_state(); + } } /* if we're no initialized, initialize the bookkeeping but do not count a failsafe */ From 1a1106878d6ab1cc48565bef9b25b07d626c5d0b Mon Sep 17 00:00:00 2001 From: Paul Riseborough Date: Fri, 15 Jun 2018 16:03:54 +1000 Subject: [PATCH 3/3] EKF: Prevent timestamp jitter failing GPS data arrival check (#462) This fixes a error condition that occurs if _time_last_gps is greater than _time_last_imu. By checking time stamps at the fusion time horizon, we guarantee that this cannot happen because all observations must have a time stamp smaller or equal to _imu_sample_delayed.time_us before they are retrieved from the buffers. --- EKF/control.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/EKF/control.cpp b/EKF/control.cpp index 46de191a3a..f704168283 100644 --- a/EKF/control.cpp +++ b/EKF/control.cpp @@ -612,7 +612,7 @@ void Ekf::controlGpsFusion() _hvelInnovGate = fmaxf(_params.vel_innov_gate, 1.0f); } - } else if (_control_status.flags.gps && (_time_last_imu - _time_last_gps > (uint64_t)10e6)) { + } else if (_control_status.flags.gps && (_imu_sample_delayed.time_us - _gps_sample_delayed.time_us > (uint64_t)10e6)) { _control_status.flags.gps = false; ECL_WARN("EKF GPS data stopped"); }