From b04a4ec04c70ded15d5df2278d5115978aef3e05 Mon Sep 17 00:00:00 2001
From: rusefi <rusefillc@gmail.com>
Date: Sun, 6 Sep 2020 23:19:32 -0400
Subject: [PATCH] microRusEFI used as Body Control Module BCM BCU

---
 firmware/console/status_loop.cpp          |  2 +-
 firmware/controllers/sensors/can_sensor.h | 24 +++++++++++------------
 firmware/init/sensor/init_can_sensors.cpp |  8 ++++----
 3 files changed, 17 insertions(+), 17 deletions(-)

diff --git a/firmware/console/status_loop.cpp b/firmware/console/status_loop.cpp
index d4e173ce2b..602ca31efd 100644
--- a/firmware/console/status_loop.cpp
+++ b/firmware/console/status_loop.cpp
@@ -467,7 +467,7 @@ extern HIP9011 instance;
 
 void updateTunerStudioState(TunerStudioOutputChannels *tsOutputChannels DECLARE_ENGINE_PARAMETER_SUFFIX) {
 #if EFI_SHAFT_POSITION_INPUT
-	int rpm = GET_RPM();
+	int rpm = Sensor::get(SensorType::Rpm).Value;
 #else /* EFI_SHAFT_POSITION_INPUT */
 	int rpm = 0;
 #endif /* EFI_SHAFT_POSITION_INPUT */
diff --git a/firmware/controllers/sensors/can_sensor.h b/firmware/controllers/sensors/can_sensor.h
index 261a99edea..57238da029 100644
--- a/firmware/controllers/sensors/can_sensor.h
+++ b/firmware/controllers/sensors/can_sensor.h
@@ -79,11 +79,11 @@ private:
 	const uint8_t m_offset;
 };
 
-template <typename TStorage, int TScale>
+template <int Size, int TScale>
 class ObdCanSensor: public CanSensorBase {
 public:
-	ObdCanSensor(uint32_t eid, int PID, SensorType type, efitick_t timeout) :
-			CanSensorBase(eid, type, timeout) {
+	ObdCanSensor(int PID, SensorType type, efitick_t timeout) :
+			CanSensorBase(OBD_TEST_RESPONSE, type, timeout) {
 		this->PID = PID;
 	}
 
@@ -91,15 +91,16 @@ public:
 		if (frame.data8[2] != PID) {
 			return;
 		}
-		// Compute the location of our data within the frame
-		const uint8_t* dataLocation = &frame.data8[0];
 
-		// Reinterpret as a scaled_channel - it already has the logic for decoding a scaled integer to a float
-		const auto scaler = reinterpret_cast<const scaled_channel<TStorage, TScale>*>(dataLocation);
+		int iValue;
+		if (Size == 2) {
+			iValue = frame.data8[3] * 256 + frame.data8[4];
+		} else {
+			iValue = frame.data8[3];
+		}
 
-		// Actually do the conversion
-		float value = *scaler;
-		setValidValue(value, nowNt);
+		float fValue = 1.0 * iValue / TScale;
+		setValidValue(fValue, nowNt);
 	}
 
 	CanSensorBase* request() override {
@@ -107,8 +108,7 @@ public:
 			CanTxMessage msg(OBD_TEST_REQUEST);
 			msg[0] = _OBD_2;
 			msg[1] = OBD_CURRENT_DATA;
-			msg[2] = getEid();
-
+			msg[2] = PID;
 		}
 		return m_next;
 	}
diff --git a/firmware/init/sensor/init_can_sensors.cpp b/firmware/init/sensor/init_can_sensors.cpp
index c621d2edcc..94a471c026 100644
--- a/firmware/init/sensor/init_can_sensors.cpp
+++ b/firmware/init/sensor/init_can_sensors.cpp
@@ -22,13 +22,13 @@ CanSensor<int16_t, PACK_MULT_PERCENT> canPedalSensor(
 	SensorType::AcceleratorPedal, TIMEOUT
 );
 
-ObdCanSensor<int16_t, ODB_RPM_MULT> obdRpmSensor(
-		OBD_TEST_RESPONSE, PID_RPM,
+ObdCanSensor<2, ODB_RPM_MULT> obdRpmSensor(
+		PID_RPM,
 	SensorType::Rpm, TIMEOUT
 );
 
-ObdCanSensor<int16_t, 1> obdCltSensor(
-		OBD_TEST_RESPONSE, PID_COOLANT_TEMP,
+ObdCanSensor<1, 1> obdCltSensor(
+		PID_COOLANT_TEMP,
 	SensorType::Clt, TIMEOUT
 );