diff --git a/src/modules/sensors/data_validator/DataValidatorGroup.cpp b/src/modules/sensors/data_validator/DataValidatorGroup.cpp index c58574307c..a7ded78d26 100644 --- a/src/modules/sensors/data_validator/DataValidatorGroup.cpp +++ b/src/modules/sensors/data_validator/DataValidatorGroup.cpp @@ -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