rewrite tps init, check if your primary/secondary sensors are too similar (#4231)

* rewrite tps init

* staticstaticstaticstaticstaticstaticstatic

firmware/controllers/sensors/sensor.h

@@ -138,7 +138,7 @@ public:
 	 * Get a friendly name for the sensor.
 	 * For example, CLT, IAT, Throttle Position 2, etc.
 	 */
-	const char* getSensorName() { return getSensorName(m_type); }
+	const char* getSensorName() const { return getSensorName(m_type); }
 	static const char* getSensorName(SensorType type);
 
 	// Retrieve the current reading from the sensor.
@@ -170,6 +170,10 @@ public:
 
 	void unregister();
 
+	SensorType type() const {
+		return m_type;
+	}
+
 protected:
 	// Protected constructor - only subclasses call this
 	explicit Sensor(SensorType type)

firmware/init/sensor/init_tps.cpp

@@ -8,114 +8,180 @@
 #include "linear_func.h"
 #include "tps.h"
 
-LinearFunc tpsFunc1p(TPS_TS_CONVERSION);
+struct TpsConfig {
-LinearFunc tpsFunc1s(TPS_TS_CONVERSION);
+	adc_channel_e channel;
-LinearFunc tpsFunc2p(TPS_TS_CONVERSION);
+	float closed;
-LinearFunc tpsFunc2s(TPS_TS_CONVERSION);
+	float open;
+	float min;
+	float max;
+};
 
-FunctionalSensor tpsSens1p(SensorType::Tps1Primary, MS2NT(10));
+class FuncSensPair {
-FunctionalSensor tpsSens1s(SensorType::Tps1Secondary, MS2NT(10));
+public:
-FunctionalSensor tpsSens2p(SensorType::Tps2Primary, MS2NT(10));
+	FuncSensPair(float divideInput, SensorType type)
-FunctionalSensor tpsSens2s(SensorType::Tps2Secondary, MS2NT(10));
+		: m_func(divideInput)
+		, m_sens(type, MS2NT(10))
+	{
+		m_sens.setFunction(m_func);
+	}
+
+	bool init(const TpsConfig& cfg) {
+		// If the configuration was invalid, don't continue to configure the sensor
+		if (!configure(cfg)) {
+			return false;
+		}
+
+		AdcSubscription::SubscribeSensor(m_sens, cfg.channel, 200);
+
+		return m_sens.Register();
+	}
+
+	void unsubscribe() {
+		AdcSubscription::UnsubscribeSensor(m_sens);
+	}
+
+	SensorType type() const {
+		return m_sens.type();
+	}
+
+	const char* name() const {
+		return m_sens.getSensorName();
+	}
+
+private:
+	bool configure(const TpsConfig& cfg) {
+		// Only configure if we have a channel
+		if (!isAdcChannelValid(cfg.channel)) {
+			return false;
+		}
+
+		float scaledClosed = cfg.closed / m_func.getDivideInput();
+		float scaledOpen = cfg.open / m_func.getDivideInput();
+
+		float split = absF(scaledOpen - scaledClosed);
+
+		// If the voltage for closed vs. open is very near, something is wrong with your calibration
+		if (split < 0.5f) {
+			firmwareError(OBD_Throttle_Position_Sensor_Circuit_Malfunction, "\"%s\" problem: open %.2f/closed %.2f cal values are too close together. Check your calibration and wiring!", name(),
+					cfg.open,
+					cfg.closed);
+			return false;
+		}
+
+		m_func.configure(
+			cfg.closed, 0,
+			cfg.open, 100, 
+			cfg.min, cfg.max
+		);
+
+		return true;
+	}
+
+	LinearFunc m_func;
+	FunctionalSensor m_sens;
+};
+
+struct RedundantPair {
+public:
+	RedundantPair(FuncSensPair& pri, FuncSensPair& sec, SensorType outputType)
+		: m_pri(pri)
+		, m_sec(sec)
+		, m_redund(outputType, m_pri.type(), m_sec.type())
+	{
+	}
+
+	void init(bool isFordTps, RedundantFordTps* fordTps, const TpsConfig& primary, const TpsConfig& secondary) {
+		{
+			// Check that the primary and secondary aren't too close together - if so, the user may have done
+			// an unsafe thing where they wired a single sensor to both inputs. Don't do that!
+			bool hasBothSensors = isAdcChannelValid(primary.channel) && isAdcChannelValid(secondary.channel);
+			bool tooCloseClosed = absF(primary.closed - secondary.closed) < 0.2f;
+			bool tooCloseOpen = absF(primary.open - secondary.open) < 0.2f;
+
+			if (hasBothSensors && tooCloseClosed && tooCloseOpen) {
+				firmwareError(OBD_Throttle_Position_Sensor_Circuit_Malfunction, "Configuration for redundant pair %s/%s are too similar - did you wire one sensor to both inputs...?", m_pri.name(), m_sec.name());
+				return;
+			}
+		}
+
+		m_pri.init(primary);
+
+		bool hasSecond = m_sec.init(secondary);
+
+		if (isFordTps && fordTps) {
+			// we have a secondary
+			fordTps->configure(5.0f, 52.6f);
+			fordTps->Register();
+		} else {
+			// not ford TPS
+			m_redund.configure(5.0f, !hasSecond);
+			m_redund.Register();
+		}
+	}
+
+	void unsubscribe() {
+		m_pri.unsubscribe();
+		m_sec.unsubscribe();
+	}
+
+private:
+	FuncSensPair& m_pri;
+	FuncSensPair& m_sec;
+
+	RedundantSensor m_redund;
+};
+
+static FuncSensPair tps1p(TPS_TS_CONVERSION, SensorType::Tps1Primary);
+static FuncSensPair tps1s(TPS_TS_CONVERSION, SensorType::Tps1Secondary);
+static FuncSensPair tps2p(TPS_TS_CONVERSION, SensorType::Tps2Primary);
+static FuncSensPair tps2s(TPS_TS_CONVERSION, SensorType::Tps2Secondary);
 
 // Used in case of "normal", non-Ford ETB TPS
-RedundantSensor tps1(SensorType::Tps1, SensorType::Tps1Primary, SensorType::Tps1Secondary);
+static RedundantPair tps1(tps1p, tps1s, SensorType::Tps1);
-RedundantSensor tps2(SensorType::Tps2, SensorType::Tps2Primary, SensorType::Tps2Secondary);
+static RedundantPair tps2(tps2p, tps2s, SensorType::Tps2);
 
 // Used only in case of weird Ford-style ETB TPS
-RedundantFordTps fordTps1(SensorType::Tps1, SensorType::Tps1Primary, SensorType::Tps1Secondary);
+static RedundantFordTps fordTps1(SensorType::Tps1, SensorType::Tps1Primary, SensorType::Tps1Secondary);
-RedundantFordTps fordTps2(SensorType::Tps2, SensorType::Tps2Primary, SensorType::Tps2Secondary);
+static RedundantFordTps fordTps2(SensorType::Tps2, SensorType::Tps2Primary, SensorType::Tps2Secondary);
 
-LinearFunc pedalFuncPrimary;
+// Pedal sensors and redundancy
-LinearFunc pedalFuncSecondary;
+static FuncSensPair pedalPrimary(1, SensorType::AcceleratorPedalPrimary);
-FunctionalSensor pedalSensorPrimary(SensorType::AcceleratorPedalPrimary, MS2NT(10));
+static FuncSensPair pedalSecondary(1, SensorType::AcceleratorPedalSecondary);
-FunctionalSensor pedalSensorSecondary(SensorType::AcceleratorPedalSecondary, MS2NT(10));
+static RedundantPair pedal(pedalPrimary, pedalSecondary, SensorType::AcceleratorPedal);
-
-RedundantSensor pedal(SensorType::AcceleratorPedal, SensorType::AcceleratorPedalPrimary, SensorType::AcceleratorPedalSecondary);
 
 // This sensor indicates the driver's throttle intent - Pedal if we have one, TPS if not.
-ProxySensor driverIntent(SensorType::DriverThrottleIntent);
+static ProxySensor driverIntent(SensorType::DriverThrottleIntent);
 
 // These sensors are TPS-like, so handle them in here too
-LinearFunc wastegateFunc(PACK_MULT_VOLTAGE);
+static FuncSensPair wastegate(PACK_MULT_VOLTAGE, SensorType::WastegatePosition);
-LinearFunc idlePosFunc(PACK_MULT_VOLTAGE);
+static FuncSensPair idlePos(PACK_MULT_VOLTAGE, SensorType::IdlePosition);
-FunctionalSensor wastegateSens(SensorType::WastegatePosition, MS2NT(10));
-FunctionalSensor idlePosSens(SensorType::IdlePosition, MS2NT(10));
-
-static bool configureTps(LinearFunc& func, adc_channel_e channel, float closed, float open, float min, float max, const char* msg) {
-	// Only configure if we have a channel
-	if (!isAdcChannelValid(channel)) {
-		return false;
-	}
-
-	float scaledClosed = closed / func.getDivideInput();
-	float scaledOpen = open / func.getDivideInput();
-
-	float split = absF(scaledOpen - scaledClosed);
-
-	// If the voltage for closed vs. open is very near, something is wrong with your calibration
-	if (split < 0.5f) {
-		firmwareError(OBD_Throttle_Position_Sensor_Circuit_Malfunction, "\"%s\" problem: open %.2f/closed %.2f cal values are too close together. Check your calibration and wiring!", msg,
-				open,
-				closed);
-		return false;
-	}
-
-	func.configure(
-		closed, 0,
-		open, 100, 
-		min, max
-	);
-
-	return true;
-}
-
-static bool initTpsFunc(LinearFunc& func, FunctionalSensor& sensor, adc_channel_e channel, float closed, float open, float min, float max) {
-	// If the configuration was invalid, don't continue to configure the sensor
-	if (!configureTps(func, channel, closed, open, min, max, sensor.getSensorName())) {
-		return false;
-	}
-
-	sensor.setFunction(func);
-
-	AdcSubscription::SubscribeSensor(sensor, channel, 200);
-
-	return sensor.Register();
-}
-
-static void initTpsFuncAndRedund(RedundantSensor& redund, RedundantFordTps* fordTps, bool isFordTps, LinearFunc& func, FunctionalSensor& sensor, adc_channel_e channel, float closed, float open, float min, float max) {
-	bool hasSecond = initTpsFunc(func, sensor, channel, closed, open, min, max);
-
-	if (isFordTps && fordTps) {
-		fordTps->configure(5.0f, 52.6f);
-		fordTps->Register();
-	} else {
-		redund.configure(5.0f, !hasSecond);
-		redund.Register();
-	}
-}
 
 void initTps() {
 	percent_t min = engineConfiguration->tpsErrorDetectionTooLow;
 	percent_t max = engineConfiguration->tpsErrorDetectionTooHigh;
 
 	if (!engineConfiguration->consumeObdSensors) {
-		// primary TPS sensors
-		initTpsFunc(tpsFunc1p, tpsSens1p, engineConfiguration->tps1_1AdcChannel, engineConfiguration->tpsMin, engineConfiguration->tpsMax, min, max);
-		initTpsFunc(tpsFunc2p, tpsSens2p, engineConfiguration->tps2_1AdcChannel, engineConfiguration->tps2Min, engineConfiguration->tps2Max, min, max);
-
-		// Secondary TPS sensors (and redundant combining)
 		bool isFordTps = engineConfiguration->useFordRedundantTps;
-		initTpsFuncAndRedund(tps1, &fordTps1, isFordTps, tpsFunc1s, tpsSens1s, engineConfiguration->tps1_2AdcChannel, engineConfiguration->tps1SecondaryMin, engineConfiguration->tps1SecondaryMax, min, max);
+
-		initTpsFuncAndRedund(tps2, &fordTps2, isFordTps, tpsFunc2s, tpsSens2s, engineConfiguration->tps2_2AdcChannel, engineConfiguration->tps2SecondaryMin, engineConfiguration->tps2SecondaryMax, min, max);
+		tps1.init(isFordTps, &fordTps1,
+			{ engineConfiguration->tps1_1AdcChannel, (float)engineConfiguration->tpsMin, (float)engineConfiguration->tpsMax, min, max },
+			{ engineConfiguration->tps1_2AdcChannel, (float)engineConfiguration->tps1SecondaryMin, (float)engineConfiguration->tps1SecondaryMax, min, max }
+		);
+
+		tps2.init(isFordTps, &fordTps2,
+			{ engineConfiguration->tps2_1AdcChannel, (float)engineConfiguration->tps2Min, (float)engineConfiguration->tps2Max, min, max },
+			{ engineConfiguration->tps2_2AdcChannel, (float)engineConfiguration->tps2SecondaryMin, (float)engineConfiguration->tps2SecondaryMax, min, max }
+		);
 
 		// Pedal sensors
-		initTpsFunc(pedalFuncPrimary, pedalSensorPrimary, engineConfiguration->throttlePedalPositionAdcChannel, engineConfiguration->throttlePedalUpVoltage, engineConfiguration->throttlePedalWOTVoltage, min, max);
+		pedal.init(false, nullptr,
-		initTpsFuncAndRedund(pedal, nullptr, false, pedalFuncSecondary, pedalSensorSecondary, engineConfiguration->throttlePedalPositionSecondAdcChannel, engineConfiguration->throttlePedalSecondaryUpVoltage, engineConfiguration->throttlePedalSecondaryWOTVoltage, min, max);
+			{ engineConfiguration->throttlePedalPositionAdcChannel, engineConfiguration->throttlePedalUpVoltage, engineConfiguration->throttlePedalWOTVoltage, min, max },
+			{ engineConfiguration->throttlePedalPositionSecondAdcChannel, engineConfiguration->throttlePedalSecondaryUpVoltage, engineConfiguration->throttlePedalSecondaryWOTVoltage, min, max }
+		);
 
 		// TPS-like stuff that isn't actually a TPS
-		initTpsFunc(wastegateFunc, wastegateSens, engineConfiguration->wastegatePositionSensor, engineConfiguration->wastegatePositionMin, engineConfiguration->wastegatePositionMax, min, max);
+		wastegate.init({ engineConfiguration->wastegatePositionSensor, (float)engineConfiguration->wastegatePositionMin, (float)engineConfiguration->wastegatePositionMax, min, max });
-		initTpsFunc(idlePosFunc, idlePosSens, engineConfiguration->idlePositionSensor, engineConfiguration->idlePositionMin, engineConfiguration->idlePositionMax, min, max);
+		idlePos.init({ engineConfiguration->idlePositionSensor, (float)engineConfiguration->idlePositionMin, (float)engineConfiguration->idlePositionMax, min, max });
 	}
 
 	// Route the pedal or TPS to driverIntent as appropriate
@@ -129,15 +195,10 @@ void initTps() {
 }
 
 void deinitTps() {
-	AdcSubscription::UnsubscribeSensor(tpsSens1p);
+	tps1.unsubscribe();
-	AdcSubscription::UnsubscribeSensor(tpsSens1s);
+	tps2.unsubscribe();
+	pedal.unsubscribe();
 
-	AdcSubscription::UnsubscribeSensor(tpsSens2p);
+	wastegate.unsubscribe();
-	AdcSubscription::UnsubscribeSensor(tpsSens2s);
+	idlePos.unsubscribe();
-
-	AdcSubscription::UnsubscribeSensor(pedalSensorPrimary);
-	AdcSubscription::UnsubscribeSensor(pedalSensorSecondary);
-
-	AdcSubscription::UnsubscribeSensor(wastegateSens);
-	AdcSubscription::UnsubscribeSensor(idlePosSens);
 }