diff --git a/firmware/controllers/sensors/redundant_sensor.cpp b/firmware/controllers/sensors/redundant_sensor.cpp index cd9f50290b..49e9cc793b 100644 --- a/firmware/controllers/sensors/redundant_sensor.cpp +++ b/firmware/controllers/sensors/redundant_sensor.cpp @@ -37,33 +37,39 @@ SensorResult RedundantSensor::get() const { return UnexpectedCode::Inconsistent; } - float sensor2Value = sensor2.Value; + if (m_secondMaximum >= 100) { + // Sensor is fully redundant - // Partial redundancy; useful for some sensors: e.g. Ford and Toyota ETCS-i - if (m_secondMaximum < 100) { - // scale the second sensor value accordingly, proportioning to the first sensor - sensor2Value *= m_secondMaximum / 100; + float delta = absF(sensor1.Value - sensor2.Value); + if (delta <= m_maxDifference) { + // All is well: sensors are valid and values check out, return the average value + return (sensor1.Value + sensor2.Value) / 2; + } + } else { + // Sensor is partially redundant; useful for some sensors: e.g. Ford and Toyota ETCS-i // The partial redundancy threshold, slightly less than 100% to avoid issues near full-range - float threshold = m_secondMaximum * 0.95; + float threshold = m_secondMaximum * 0.95f; + + // Scale the second sensor value accordingly, proportioning to the first sensor + float scaledSecond = sensor2.Value * m_secondMaximum / 100; // Check second sensor is below partial redundancy switch-over threshold - if (sensor2Value > threshold) { - if (sensor1.Value >= threshold - m_maxDifference) { + if (scaledSecond <= threshold) { + float delta = absF(sensor1.Value - scaledSecond); + if (delta <= m_maxDifference) { + // All is well: sensors are valid and values check out, return the primary value + return sensor1.Value; + } + } else { + // Check first sensor is at or above partial redundancy switch-over threshold + if (sensor1.Value >= m_secondMaximum - m_maxDifference) { + // All is well: sensors are valid and values check out, return the primary value return sensor1.Value; } - - // There's a discrepancy: first sensor is out of compliance, return inconsistency error - return UnexpectedCode::Inconsistent; } } - float delta = absF(sensor1.Value - sensor2Value); - if (delta <= m_maxDifference) { - // All is well: sensors are valid and values check out - return (sensor1.Value + sensor2Value) / 2; - } - // Any other condition indacates an unexpected discrepancy, return inconsistency error return UnexpectedCode::Inconsistent; }