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

#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;
}