54 lines
1.6 KiB
C++
54 lines
1.6 KiB
C++
#include "pch.h"
|
|
|
|
#include "redundant_ford_tps.h"
|
|
|
|
RedundantFordTps::RedundantFordTps(SensorType outputType, SensorType first, SensorType second)
|
|
: Sensor(outputType)
|
|
, m_first(first)
|
|
, m_second(second)
|
|
{
|
|
}
|
|
|
|
void RedundantFordTps::configure(float maxDifference, float secondaryMaximum) {
|
|
m_maxDifference = maxDifference;
|
|
m_secondaryMaximum = secondaryMaximum;
|
|
}
|
|
|
|
SensorResult RedundantFordTps::get() const {
|
|
// Sensor 1 is "plain linear" and has the full range
|
|
auto tps1 = Sensor::get(m_first);
|
|
|
|
// Sensor 2 is compressed in to 0 -> 50%
|
|
auto tps2 = Sensor::get(m_second);
|
|
|
|
// If either result is invalid, return invalid.
|
|
if (!tps1 || !tps2) {
|
|
return UnexpectedCode::Inconsistent;
|
|
}
|
|
|
|
// The "actual" position resolved by the second throttle - this tops out at m_secondaryMaximum instead of 100%
|
|
float tps2Actual = tps2.Value * m_secondaryMaximum / 100;
|
|
|
|
// Switch modes slightly below the maximum of the secondary, to avoid misbehavior near 100%
|
|
float avgThreshold = m_secondaryMaximum * 0.9f;
|
|
|
|
// Case 1: both sensors show low position, average result
|
|
if (tps1.Value < avgThreshold && tps2Actual < avgThreshold) {
|
|
// Check that the resolved positions are close
|
|
float delta = absF(tps1.Value - tps2Actual);
|
|
if (delta > m_maxDifference) {
|
|
return UnexpectedCode::Inconsistent;
|
|
}
|
|
|
|
return (tps1.Value + tps2Actual) / 2;
|
|
}
|
|
|
|
// Case 2: both sensors show high position, return "full scale" sensor's position
|
|
if (tps1.Value > avgThreshold && tps2Actual > (avgThreshold - 3)) {
|
|
return tps1;
|
|
}
|
|
|
|
// Any other condition indicates an mismatch, and therefore an error
|
|
return UnexpectedCode::Inconsistent;
|
|
}
|