adc resubscription of the same sensor (#4947)

* adc resubscription of the same sensor

* overload function name

* fix logic

* thermistors and map

firmware/hw_layer/adc/adc_inputs.cpp

@@ -398,10 +398,6 @@ void addChannel(const char *name, adc_channel_e setting, adc_channel_mode_e mode
 	if (!isAdcChannelValid(setting)) {
 		return;
 	}
-	if (/*type-limited (int)setting < 0 || */(int)setting>=HW_MAX_ADC_INDEX) {
-		firmwareError(CUSTOM_INVALID_ADC, "Invalid ADC setting %s", name);
-		return;
-	}
 
 	adcHwChannelEnabled[setting] = mode;
 

firmware/hw_layer/adc/adc_subscription.cpp

@@ -12,6 +12,9 @@
 /*static*/ void AdcSubscription::UnsubscribeSensor(FunctionalSensor&) {
 }
 
+/*static*/ void AdcSubscription::UnsubscribeSensor(FunctionalSensor&, adc_channel_e) {
+}
+
 #else
 
 struct AdcSubscriptionEntry {
@@ -48,17 +51,27 @@ static AdcSubscriptionEntry* findEntry() {
 		return;
 	}
 
-	const char* name = sensor.getSensorName();
-	if (/*type-limited (int)setting < 0 || */(int)channel >= HW_MAX_ADC_INDEX) {
-		firmwareError(CUSTOM_INVALID_ADC, "Invalid ADC setting %s", name);
-		return;
+	// If you passed the same sensor again, resubscribe it with the new parameters
+	auto entry = findEntry(&sensor);
+
+	if (entry) {
+		// If the channel didn't change, we're already set
+		if (entry->Channel == channel) {
+			return;
+		}
+
+		// avoid updates to this while we're mucking with the configuration
+		entry->Sensor = nullptr;
+	} else {
+		// If not already registered, get an empty (new) entry
+		entry = findEntry();
 	}
 
-	auto entry = findEntry();
+	const char* name = sensor.getSensorName();
 
 	// Ensure that a free entry was found
 	if (!entry) {
-		firmwareError(CUSTOM_INVALID_ADC, "too many ADC subscriptions");
+		firmwareError(CUSTOM_INVALID_ADC, "too many ADC subscriptions subscribing %s", name);
 		return;
 	}
 
@@ -104,6 +117,16 @@ static AdcSubscriptionEntry* findEntry() {
 	entry->Channel = EFI_ADC_NONE;
 }
 
+/*static*/ void AdcSubscription::UnsubscribeSensor(FunctionalSensor& sensor, adc_channel_e channel) {
+	// Find the old sensor
+	auto entry = findEntry(&sensor);
+
+	// if the channel changed, unsubscribe!
+	if (entry && entry->Channel != channel) {
+		AdcSubscription::UnsubscribeSensor(sensor);
+	}
+}
+
 void AdcSubscription::UpdateSubscribers(efitick_t nowNt) {
 	ScopePerf perf(PE::AdcSubscriptionUpdateSubscribers);
 

firmware/hw_layer/adc/adc_subscription.h

@@ -11,6 +11,7 @@ class AdcSubscription {
 public:
 	static void SubscribeSensor(FunctionalSensor &sensor, adc_channel_e channel, float lowpassCutoff, float voltsPerAdcVolt = 0.0f);
 	static void UnsubscribeSensor(FunctionalSensor& sensor);
+	static void UnsubscribeSensor(FunctionalSensor& sensor, adc_channel_e newChannel);
 	static void UpdateSubscribers(efitick_t nowNt);
 
 	static void PrintInfo();

firmware/init/sensor/init_fluid_pressure.cpp

@@ -54,9 +54,7 @@ static void initFluidPressure(LinearFunc& func, FunctionalSensor& sensor, const
 void initFluidPressure() {
 	initFluidPressure(oilpSensorFunc, oilpSensor, engineConfiguration->oilPressure, 10);
 	initFluidPressure(fuelPressureFuncLow, fuelPressureSensorLow, engineConfiguration->lowPressureFuel, 10);
-	if (isConfigurationChanged(highPressureFuel.hwChannel)) {
-	    initFluidPressure(fuelPressureFuncHigh, fuelPressureSensorHigh, engineConfiguration->highPressureFuel, 100);
-	}
+	initFluidPressure(fuelPressureFuncHigh, fuelPressureSensorHigh, engineConfiguration->highPressureFuel, 100);
 	initFluidPressure(auxLinear1Func, auxLinear1Sensor, engineConfiguration->auxLinear1, 10);
 	initFluidPressure(auxLinear2Func, auxLinear2Sensor, engineConfiguration->auxLinear2, 10);
 
@@ -70,11 +68,9 @@ void initFluidPressure() {
 }
 
 void deinitFluidPressure() {
-	AdcSubscription::UnsubscribeSensor(oilpSensor);
-	AdcSubscription::UnsubscribeSensor(fuelPressureSensorLow);
-	if (isConfigurationChanged(highPressureFuel.hwChannel)) {
-	    AdcSubscription::UnsubscribeSensor(fuelPressureSensorHigh);
-	}
-	AdcSubscription::UnsubscribeSensor(auxLinear1Sensor);
-	AdcSubscription::UnsubscribeSensor(auxLinear2Sensor);
+	AdcSubscription::UnsubscribeSensor(oilpSensor, engineConfiguration->oilPressure.hwChannel);
+	AdcSubscription::UnsubscribeSensor(fuelPressureSensorLow, engineConfiguration->lowPressureFuel.hwChannel);
+	AdcSubscription::UnsubscribeSensor(fuelPressureSensorHigh, engineConfiguration->highPressureFuel.hwChannel);
+	AdcSubscription::UnsubscribeSensor(auxLinear1Sensor, engineConfiguration->auxLinear1.hwChannel);
+	AdcSubscription::UnsubscribeSensor(auxLinear2Sensor, engineConfiguration->auxLinear2.hwChannel);
 }

firmware/init/sensor/init_map.cpp

@@ -129,6 +129,6 @@ void initMap() {
 }
 
 void deinitMap() {
-	AdcSubscription::UnsubscribeSensor(slowMapSensor);
-	AdcSubscription::UnsubscribeSensor(baroSensor);
+	AdcSubscription::UnsubscribeSensor(slowMapSensor, engineConfiguration->map.sensor.hwChannel);
+	AdcSubscription::UnsubscribeSensor(baroSensor, engineConfiguration->baroSensor.hwChannel);
 }

firmware/init/sensor/init_thermistors.cpp

@@ -112,8 +112,8 @@ void initThermistors() {
 }
 
 void deinitThermistors() {
-	AdcSubscription::UnsubscribeSensor(clt);
-	AdcSubscription::UnsubscribeSensor(iat);
-	AdcSubscription::UnsubscribeSensor(aux1);
-	AdcSubscription::UnsubscribeSensor(aux2);
+	AdcSubscription::UnsubscribeSensor(clt, engineConfiguration->clt.adcChannel);
+	AdcSubscription::UnsubscribeSensor(iat, engineConfiguration->iat.adcChannel);
+	AdcSubscription::UnsubscribeSensor(aux1, engineConfiguration->auxTempSensor1.adcChannel);
+	AdcSubscription::UnsubscribeSensor(aux2, engineConfiguration->auxTempSensor2.adcChannel);
 }

firmware/init/sensor/init_vbatt.cpp

@@ -23,5 +23,5 @@ void initVbatt() {
 }
 
 void deinitVbatt() {
-	AdcSubscription::UnsubscribeSensor(vbattSensor);
+	AdcSubscription::UnsubscribeSensor(vbattSensor, engineConfiguration->vbattAdcChannel);
 }