OBD CAN sensors #1733

firmware/controllers/can/can_rx.cpp

@@ -43,6 +43,7 @@ void serviceCanSubscribers(const CANRxFrame& frame, efitick_t nowNt) {
 void registerCanSensor(CanSensorBase& sensor) {
 	sensor.setNext(cansensors_head);
 	cansensors_head = &sensor;
+	sensor.Register();
 }
 
 void processCanRxMessage(const CANRxFrame& frame, Logging* logger, efitick_t nowNt) {

firmware/controllers/can/obd2.h

@@ -43,4 +43,4 @@
 void obdOnCanPacketRx(const CANRxFrame& rx);
 #endif /* HAL_USE_CAN */
 
-#define ODB_RPM_MULT 4.0f
+#define ODB_RPM_MULT 4

firmware/controllers/sensors/can_sensor.h

@@ -10,6 +10,8 @@
 #include "stored_value_sensor.h"
 #include "scaled_channel.h"
 #include "hal.h"
+#include "can_msg_tx.h"
+#include "obd2.h"
 
 /**
  * Sensor which reads it's value from CAN
@@ -36,15 +38,19 @@ public:
 		return m_next;
 	}
 
+	uint32_t getEid() {
+		return m_eid;
+	}
+
 	void setNext(CanSensorBase* next) {
 		m_next = next;
 	}
 
 protected:
 	virtual void decodeFrame(const CANRxFrame& frame, efitick_t nowNt) = 0;
+	CanSensorBase* m_next = nullptr;
 
 private:
-	CanSensorBase* m_next = nullptr;
 	const uint32_t m_eid;
 };
 
@@ -73,7 +79,34 @@ private:
 	const uint8_t m_offset;
 };
 
-class ObdCanSensor : public CanSensorBase {
+template <typename TStorage, int TScale>
-	ObdCanSensor(SensorType type);
+class ObdCanSensor: public CanSensorBase {
+public:
+	ObdCanSensor(uint32_t eid, SensorType type, efitick_t timeout) :
+			CanSensorBase(eid, type, timeout) {
+	}
+
+	void decodeFrame(const CANRxFrame& frame, efitick_t nowNt) override {
+		// 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);
+
+		// Actually do the conversion
+		float value = *scaler;
+		setValidValue(value, nowNt);
+	}
+
+	CanSensorBase* request() override {
+		{
+			CanTxMessage msg(OBD_TEST_REQUEST);
+			msg[0] = _OBD_2;
+			msg[1] = OBD_CURRENT_DATA;
+			msg[2] = getEid();
+
+		}
+		return m_next;
+	}
 };
 

firmware/controllers/sensors/sensor.cpp

@@ -1,3 +1,4 @@
+#include "global.h"
 #include "sensor.h"
 #include "efilib.h"
 #include "loggingcentral.h"
@@ -84,6 +85,7 @@ bool Sensor::Register() {
 	// If there's somebody already here - a consumer tried to double-register a sensor
 	if (entry.getSensor()) {
 		// This sensor has already been registered. Don't re-register it.
+		firmwareError(CUSTOM_OBD_26, "Duplicate registration for %s sensor", s_sensorNames[getIndex()]);
 		return false;
 	} else {
 		// put ourselves in the registry

firmware/init/sensor/init_can_sensors.cpp

@@ -11,18 +11,23 @@
 #include "can_sensor.h"
 #include "can.h"
 
+#define TIMEOUT MS2NT(100)
+
 CanSensor<int16_t, PACK_MULT_PERCENT> canPedalSensor(
 	CAN_DEFAULT_BASE + CAN_PEDAL_TPS_OFFSET, /*offset =*/ 0,
-	SensorType::AcceleratorPedal, MS2NT(100)
+	SensorType::AcceleratorPedal, TIMEOUT
+);
+
+ObdCanSensor<int16_t, ODB_RPM_MULT> obdCltSensor(
+	PID_RPM,
+	SensorType::Rpm, TIMEOUT
 );
 
 void initCanSensors() {
 #if EFI_CANBUS_SLAVE
 	registerCanSensor(canPedalSensor);
+	registerCanSensor(obdCltSensor);
 
-	if (!canPedalSensor.Register()) {
-		firmwareError(CUSTOM_INVALID_TPS_SETTING, "Duplicate registration for pedal sensor");
-	}
 #endif // EFI_CANBUS_SLAVE
 }
 #endif // EFI_CAN_SUPPORT