From 0f489a194fe066848fe92add6cdcdeeca143d0d3 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Thu, 12 Nov 2015 12:18:19 +0530 Subject: [PATCH 1/4] attitude_q : more verbose failsafe output --- .../ecl/validation/data_validator_group.cpp | 2 +- .../attitude_estimator_q_main.cpp | 23 +++++++++++++++---- 2 files changed, 19 insertions(+), 6 deletions(-) diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index ac4bcc8b9b..e82e476416 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -199,7 +199,7 @@ void DataValidatorGroup::print() { /* print the group's state */ - ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (# %u)", + ECL_INFO("validator: best: %d, prev best: %d, failsafe: %s (%u events)", _curr_best, _prev_best, (_toggle_count > 0) ? "YES" : "NO", _toggle_count); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index 1531e32778..b62c3b6cc5 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -369,12 +369,25 @@ void AttitudeEstimatorQ::task_main() _data_good = true; - if (!_failsafe && (_voter_gyro.failover_count() > 0 || - _voter_accel.failover_count() > 0 || - _voter_mag.failover_count() > 0)) { + if (!_failsafe) { + if (_voter_gyro.failover_count() > 0) { + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "Gyro failure!"); + } - _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + if (_voter_accel.failover_count() > 0) { + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "Accel failure!"); + } + + if (_voter_mag.failover_count() > 0) { + _failsafe = true; + mavlink_and_console_log_emergency(_mavlink_fd, "Mag failure!"); + } + + if (_failsafe) { + mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY"); + } } if (!_vibration_warning && (_voter_gyro.get_vibration_factor(curr_time) > _vibration_warning_threshold || From 2d776aca14fdd2bcb97b80cee4e22dae50f90d5e Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Thu, 12 Nov 2015 14:50:49 +0530 Subject: [PATCH 2/4] attitude_q : ignore blank fields --- .../attitude_estimator_q_main.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index b62c3b6cc5..bbdf3ccc85 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -347,11 +347,16 @@ void AttitudeEstimatorQ::task_main() _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); } - - _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], - sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); - _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], + /* ignore empty fields */ + if (sensors.accelerometer_timestamp[i] > 0) { + _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], + sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + } + /* ignore empty fields */ + if (sensors.magnetometer_timestamp[i] > 0) { + _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + } } int best_gyro, best_accel, best_mag; From 0d7cd22ae7f67452ed1fb1f9ed5c89a64fc1ee37 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Sat, 14 Nov 2015 11:58:51 +0530 Subject: [PATCH 3/4] data validator : verbose error state reporting --- src/lib/ecl/validation/data_validator.cpp | 45 +++++++++-------- src/lib/ecl/validation/data_validator.h | 24 +++++++++- .../ecl/validation/data_validator_group.cpp | 48 +++++++++++++++++-- src/lib/ecl/validation/data_validator_group.h | 14 ++++++ 4 files changed, 108 insertions(+), 23 deletions(-) diff --git a/src/lib/ecl/validation/data_validator.cpp b/src/lib/ecl/validation/data_validator.cpp index 737d9ea1a3..299811ea43 100644 --- a/src/lib/ecl/validation/data_validator.cpp +++ b/src/lib/ecl/validation/data_validator.cpp @@ -43,6 +43,7 @@ #include DataValidator::DataValidator(DataValidator *prev_sibling) : + _error_mask(ERROR_FLAG_NO_ERROR), _time_last(0), _timeout_interval(20000), _event_count(0), @@ -111,39 +112,45 @@ DataValidator::put(uint64_t timestamp, float val[3], uint64_t error_count_in, in float DataValidator::confidence(uint64_t timestamp) { + float ret = 1.0f; + /* check if we have any data */ if (_time_last == 0) { - return 0.0f; + _error_mask |= ERROR_FLAG_NO_DATA; + ret = 0.0f; } - - /* check error count limit */ - if (_error_count > NORETURN_ERRCOUNT) { - return 0.0f; + + /* timed out - that's it */ + if (timestamp - _time_last > _timeout_interval) { + _error_mask |= ERROR_FLAG_TIMEOUT; + ret = 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; + _error_mask |= ERROR_FLAG_STALE_DATA; + ret = 0.0f; } - - /* timed out - that's it */ - if (timestamp - _time_last > _timeout_interval) { - return 0.0f; + + /* check error count limit */ + if (_error_count > NORETURN_ERRCOUNT) { + _error_mask |= ERROR_FLAG_HIGH_ERRCOUNT; + ret = 0.0f; } /* cap error density counter at window size */ if (_error_density > ERROR_DENSITY_WINDOW) { + _error_mask |= ERROR_FLAG_HIGH_ERRDENSITY; _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; + + /* no critical errors */ + if(ret > 0.0f) { + /* return local error density for last N measurements */ + ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW); + } + + return ret; } void diff --git a/src/lib/ecl/validation/data_validator.h b/src/lib/ecl/validation/data_validator.h index dde9cb51aa..31f6d9d989 100644 --- a/src/lib/ecl/validation/data_validator.h +++ b/src/lib/ecl/validation/data_validator.h @@ -91,7 +91,18 @@ public: * Get the priority of this validator * @return the stored priority */ - int priority(); + int priority() { return (_priority); } + + /** + * Get the error state of this validator + * @return the bitmask with the error status + */ + uint32_t state() { return (_error_mask); } + + /** + * Reset the error state of this validator + */ + void reset_state() { _error_mask = ERROR_FLAG_NO_ERROR; } /** * Get the RMS values of this validator @@ -111,9 +122,20 @@ public: * @param timeout_interval_us The timeout interval in microseconds */ void set_timeout(uint64_t timeout_interval_us) { _timeout_interval = timeout_interval_us; } + + /** + * 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); private: static const unsigned _dimensions = 3; + uint32_t _error_mask; /**< sensor error state */ 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 */ diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index e82e476416..1a22672181 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -138,11 +138,13 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index) bool true_failsafe = true; - /* check wether the switch was a failsafe or preferring a higher priority sensor */ + /* 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 we're no initialized, initialize the bookkeeping but do not count a failsafe */ @@ -203,13 +205,21 @@ DataValidatorGroup::print() _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()); + uint32_t flags = next->state(); + + ECL_INFO("sensor #%u, prio: %d, state:%s%s%s%s%s%s", i, next->priority(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " NO_DATA" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE_DATA" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " DATA_TIMEOUT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " HIGH_ERRCOUNT" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " HIGH_ERRDENSITY" : ""), + ((flags == DataValidator::ERROR_FLAG_NO_ERROR) ? " OK" : "")); + next->print(); } next = next->sibling(); @@ -222,3 +232,35 @@ DataValidatorGroup::failover_count() { return _toggle_count; } + +int +DataValidatorGroup::failover_index() +{ + DataValidator *next = _first; + unsigned i = 0; + + while (next != nullptr) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) { + return i; + } + next = next->sibling(); + i++; + } + return -1; +} + +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 == _prev_best)) { + return next->state(); + } + next = next->sibling(); + i++; + } + return DataValidator::ERROR_FLAG_NO_ERROR; +} diff --git a/src/lib/ecl/validation/data_validator_group.h b/src/lib/ecl/validation/data_validator_group.h index 3756be2638..855c3ed4ae 100644 --- a/src/lib/ecl/validation/data_validator_group.h +++ b/src/lib/ecl/validation/data_validator_group.h @@ -80,6 +80,20 @@ public: * @return the number of failovers */ unsigned failover_count(); + + /** + * Get the index of the failed sensor in the group + * + * @return index of the failed sensor + */ + 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(); /** * Print the validator value From 5a1f7ca95a29f53eaad39f774b0f9c2b66784179 Mon Sep 17 00:00:00 2001 From: Kabir Mohammed Date: Sat, 14 Nov 2015 11:58:34 +0530 Subject: [PATCH 4/4] attitude_q : verbose failure reporting --- .../ecl/validation/data_validator_group.cpp | 4 +-- .../attitude_estimator_q_main.cpp | 35 ++++++++++++++++--- 2 files changed, 32 insertions(+), 7 deletions(-) diff --git a/src/lib/ecl/validation/data_validator_group.cpp b/src/lib/ecl/validation/data_validator_group.cpp index 1a22672181..8bf060cf65 100644 --- a/src/lib/ecl/validation/data_validator_group.cpp +++ b/src/lib/ecl/validation/data_validator_group.cpp @@ -240,7 +240,7 @@ DataValidatorGroup::failover_index() unsigned i = 0; while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { return i; } next = next->sibling(); @@ -256,7 +256,7 @@ DataValidatorGroup::failover_state() unsigned i = 0; while (next != nullptr) { - if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == _prev_best)) { + if (next->used() && (next->state() != DataValidator::ERROR_FLAG_NO_ERROR) && (i == (unsigned)_prev_best)) { return next->state(); } next = next->sibling(); diff --git a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp index bbdf3ccc85..4b4fba54c8 100644 --- a/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp +++ b/src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp @@ -347,15 +347,17 @@ void AttitudeEstimatorQ::task_main() _voter_gyro.put(i, sensors.gyro_timestamp[i], &gyro[0], sensors.gyro_errcount[i], sensors.gyro_priority[i]); } + /* ignore empty fields */ if (sensors.accelerometer_timestamp[i] > 0) { _voter_accel.put(i, sensors.accelerometer_timestamp[i], &sensors.accelerometer_m_s2[i * 3], - sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); + sensors.accelerometer_errcount[i], sensors.accelerometer_priority[i]); } + /* ignore empty fields */ if (sensors.magnetometer_timestamp[i] > 0) { _voter_mag.put(i, sensors.magnetometer_timestamp[i], &sensors.magnetometer_ga[i * 3], - sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); + sensors.magnetometer_errcount[i], sensors.magnetometer_priority[i]); } } @@ -375,19 +377,42 @@ void AttitudeEstimatorQ::task_main() _data_good = true; if (!_failsafe) { + uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR; + if (_voter_gyro.failover_count() > 0) { _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "Gyro failure!"); + flags = _voter_gyro.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Gyro #%i failure :%s%s%s%s%s!", + _voter_gyro.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); } if (_voter_accel.failover_count() > 0) { _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "Accel failure!"); + flags = _voter_accel.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Accel #%i failure :%s%s%s%s%s!", + _voter_accel.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); } if (_voter_mag.failover_count() > 0) { _failsafe = true; - mavlink_and_console_log_emergency(_mavlink_fd, "Mag failure!"); + flags = _voter_mag.failover_state(); + mavlink_and_console_log_emergency(_mavlink_fd, "Mag #%i failure :%s%s%s%s%s!", + _voter_mag.failover_index(), + ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " No data" : ""), + ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " Stale data" : ""), + ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " Data timeout" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " High error count" : ""), + ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " High error density" : "")); } if (_failsafe) {