fome-fw/firmware/controllers/sensors/redundant_sensor.cpp

76 lines
2.4 KiB
C++

#include "pch.h"
#include "redundant_sensor.h"
RedundantSensor::RedundantSensor(SensorType outputType, SensorType first, SensorType second)
: Sensor(outputType)
, m_first(first)
, m_second(second)
{
}
void RedundantSensor::configure(float maxDifference, bool ignoreSecondSensor, float secondMaximum) {
m_maxDifference = maxDifference;
m_ignoreSecond = ignoreSecondSensor;
m_secondMaximum = secondMaximum;
}
SensorResult RedundantSensor::get() const {
// Sensor 1 is always full range, i.e. 0% -> 100%
auto sensor1 = Sensor::get(m_first);
// If we're set to disable redundancy, just pass thru the first sensor
if (m_ignoreSecond) {
return sensor1;
}
// If sensor 1 result is invalid, return inconsistency error
if (!sensor1.Valid) {
return UnexpectedCode::Inconsistent;
}
// Sensor 2 may be partial, i.e. 0% -> m_secondMaximum%
auto sensor2 = Sensor::get(m_second);
// If sensor 2 result is invalid, return inconsistency error
if (!sensor2.Valid) {
return UnexpectedCode::Inconsistent;
}
if (m_secondMaximum >= 100) {
// Sensor is fully redundant
float delta = std::abs(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.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 (scaledSecond <= threshold) {
float delta = std::abs(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;
}
}
}
// Any other condition indicates an unexpected discrepancy, return inconsistency error
return UnexpectedCode::Inconsistent;
}