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

DataValidator: fix unexpected failover 

Instead of always starting with instance 0 (potentially an internal
mag), first take the current sensor as reference to compare the other
ones against it.
The issue is that otherwise we can end up in a
situation where a switch occurs because the currently used sensor isn't
much better than a sensor with a lower index: because the selected one
isn't much better, we cannot "fail-over" to it and get stuck on another
sensor, triggering a fail-over.
This commit is contained in:
bresch 2022-12-05 14:13:09 +01:00 committed by Beat Küng
parent c5dc1221b6
commit 8e3517fae0
1 changed files with 23 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
src/modules/sensors/data_validator/DataValidatorGroup.cpp
View File

@ -153,14 +153,32 @@ float *DataValidatorGroup::get_best(uint64_t timestamp, int *index)
int i = 0;
// First find the current selected sensor
while (next != nullptr) {
if (i == pre_check_best) {
const int prio = next->priority();
const float confidence = next->confidence(timestamp);
pre_check_prio = prio;
pre_check_confidence = confidence;
max_index = i;
max_confidence = confidence;
max_priority = prio;
best = next;
break;
}
next = next->sibling();
i++;
}
i = 0;
next = _first;
while (next != nullptr) {
float confidence = next->confidence(timestamp);
if (i == pre_check_best) {
pre_check_prio = next->priority();
pre_check_confidence = confidence;
}
/*
* Switch if:
* 1) the confidence is higher and priority is equal or higher