fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #3183 from mhkabir/uavcan_voting

Browse Source 
Improved sensor failsafe reporting
This commit is contained in:
Lorenz Meier 2015-11-14 18:02:25 +01:00
commit ee2ce77c82
5 changed files with 161 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

45
src/lib/ecl/validation/data_validator.cpp
View File

@ -43,6 +43,7 @@
#include <ecl/ecl.h>
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

24
src/lib/ecl/validation/data_validator.h
View File

@ -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 */

50
src/lib/ecl/validation/data_validator_group.cpp
View File

@ -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 */
@ -199,17 +201,25 @@ 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);
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 == (unsigned)_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 == (unsigned)_prev_best)) {
return next->state();
}
next = next->sibling();
i++;
}
return DataValidator::ERROR_FLAG_NO_ERROR;
}

14
src/lib/ecl/validation/data_validator_group.h
View File

@ -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

61
src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp
View File

@ -348,10 +348,17 @@ 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],
sensors.magnetometer_errcount[i], sensors.magnetometer_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]);
}
/* 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;
@ -369,12 +376,48 @@ 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) {
uint32_t flags = DataValidator::ERROR_FLAG_NO_ERROR;
_failsafe = true;
mavlink_and_console_log_emergency(_mavlink_fd, "SENSOR FAILSAFE! RETURN TO LAND IMMEDIATELY");
if (_voter_gyro.failover_count() > 0) {
_failsafe = true;
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;
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;
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) {
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 ||