From b8a881855463c2f4f0c87240425536654c7d9aba Mon Sep 17 00:00:00 2001
From: rusefi <arro239+rusefi@gmail.com>
Date: Fri, 5 May 2017 21:05:59 -0400
Subject: [PATCH] digital input stop

---
 firmware/hw_layer/digital_input_hw.cpp | 23 +++++++++++++++++++++++
 firmware/hw_layer/digital_input_hw.h   |  1 +
 firmware/hw_layer/vehicle_speed.cpp    |  3 +--
 3 files changed, 25 insertions(+), 2 deletions(-)

diff --git a/firmware/hw_layer/digital_input_hw.cpp b/firmware/hw_layer/digital_input_hw.cpp
index bd0b2ed4f2..e8d6ab675c 100644
--- a/firmware/hw_layer/digital_input_hw.cpp
+++ b/firmware/hw_layer/digital_input_hw.cpp
@@ -189,12 +189,35 @@ digital_input_s * initWaveAnalyzerDriver(const char *msg, brain_pin_e brainPin)
 	digital_input_s *hw = registeredIcus.add();
 	hw->widthListeners.clear();
 	hw->periodListeners.clear();
+	hw->started = false;
 	hw->brainPin = brainPin;
 	hw->driver = driver;
 	turnOnCapturePin(msg, brainPin);
 	return hw;
 }
 
+void stopWaveAnalyzerDriver(const char *msg, brain_pin_e brainPin) {
+	if (brainPin == GPIO_UNASSIGNED) {
+		return;
+	}
+	unmarkPin(brainPin);
+
+	ICUDriver *driver = getInputCaptureDriver(msg, brainPin);
+	if (driver == NULL) {
+		return;
+	}
+	int regSize = registeredIcus.size;
+	for (int i = 0; i < regSize; i++) {
+		if (registeredIcus.elements[i].driver == driver) {
+			// removing from driver from the list of used drivers
+			memcpy(&registeredIcus.elements[i], &registeredIcus.elements[regSize - 1],
+				sizeof(digital_input_s));
+			registeredIcus.size--;
+			return;
+		}
+	}
+}
+
 void startInputDriver(digital_input_s *hw, bool isActiveHigh) {
 	hw->isActiveHigh = isActiveHigh;
 	if (hw->isActiveHigh) {
diff --git a/firmware/hw_layer/digital_input_hw.h b/firmware/hw_layer/digital_input_hw.h
index 6411ac8ad6..af70656293 100644
--- a/firmware/hw_layer/digital_input_hw.h
+++ b/firmware/hw_layer/digital_input_hw.h
@@ -28,6 +28,7 @@ digital_input_s *initWaveAnalyzerDriver(const char *msg, brain_pin_e brainPin);
 void startInputDriver(digital_input_s *hw, bool isActiveHigh);
 ICUDriver * getInputCaptureDriver(const char *msg, brain_pin_e hwPin);
 icuchannel_t getInputCaptureChannel(brain_pin_e hwPin);
+void stopWaveAnalyzerDriver(const char *msg, brain_pin_e brainPin);
 
 #endif
 
diff --git a/firmware/hw_layer/vehicle_speed.cpp b/firmware/hw_layer/vehicle_speed.cpp
index 6869540fcb..813a75894a 100644
--- a/firmware/hw_layer/vehicle_speed.cpp
+++ b/firmware/hw_layer/vehicle_speed.cpp
@@ -67,8 +67,7 @@ bool hasVehicleSpeedSensor() {
 }
 
 void stopVSSPins(void) {
-	unmarkPin(activeConfiguration.bc.vehicleSpeedSensorInputPin);
-	// todo: remove driver from registeredIcus
+	stopWaveAnalyzerDriver("VSS", activeConfiguration.bc.vehicleSpeedSensorInputPin);
 }
 
 void startVSSPins(void) {