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

Update validator to provide correct response for single sensor usage

This commit is contained in:
Lorenz Meier 2016-12-17 20:44:46 +01:00
parent aad3a2b751
commit 6f78ef4084
2 changed files with 11 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
validation/data_validator.cpp
View File

@ -151,15 +151,16 @@ DataValidator::confidence(uint64_t timestamp)
_error_mask |= ERROR_FLAG_HIGH_ERRDENSITY;
_error_density = ERROR_DENSITY_WINDOW;
/* no error */
} else {
_error_mask = ERROR_FLAG_NO_ERROR;
}
/* no critical errors */
if (ret > 0.0f) {
/* return local error density for last N measurements */
ret = 1.0f - (_error_density / ERROR_DENSITY_WINDOW);
if (ret > 0.0f) {
_error_mask = ERROR_FLAG_NO_ERROR;
}
}
return ret;

15
validation/data_validator_group.cpp
View File

@ -148,10 +148,10 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
* 1) the confidence is higher and priority is equal or higher
* 2) the confidence is no less than 1% different and the priority is higher
*/
if (((max_confidence < MIN_REGULAR_CONFIDENCE) && (confidence >= MIN_REGULAR_CONFIDENCE)) ||
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 > FLT_EPSILON)) {
max_index = i;
max_confidence = confidence;
max_priority = next->priority();
@ -162,8 +162,9 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
i++;
}
/* the current best sensor is not matching the previous best sensor */
if (max_index != _curr_best) {
/* 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;
@ -193,10 +194,8 @@ DataValidatorGroup::get_best(uint64_t timestamp, int *index)
}
}
if (max_index >= 0) {
/* for all cases we want to keep a record of the best index */
_curr_best = max_index;
}
/* for all cases we want to keep a record of the best index */
_curr_best = max_index;
}
*index = max_index;
return (best) ? best->value() : nullptr;