Sensor-via-CAN (#1241)

* CAN sensor impl

* subscription hookup

* printing

* init

* inject stamp

* don't init const field

* clarify

* guard for non-CAN builds

* fix merge

* guard with EFI_CANBUS_SLAVE

* headers, guards

* include

Co-authored-by: Matthew Kennedy <makenne@microsoft.com>

firmware/controllers/can/can.h

@@ -15,7 +15,10 @@
 #define CAN_SENSOR_1_OFFSET 3
 
 class Logging;
-void processCanRxMessage(const CANRxFrame& msg, Logging* logger);
+class CanSensorBase;
+
+void processCanRxMessage(const CANRxFrame& msg, Logging* logger, efitick_t nowNt);
+void registerCanSensor(CanSensorBase& sensor);
 
 class CanWrite final : public PeriodicController<512> {
 public:

firmware/controllers/can/can_rx.cpp

@@ -14,6 +14,7 @@
 #include "can.h"
 #include "obd2.h"
 #include "engine.h"
+#include "can_sensor.h"
 
 EXTERN_ENGINE;
 
@@ -29,11 +30,28 @@ volatile float aemXSeriesLambda = 0;
 volatile float canPedal = 0;
 volatile float canMap = 0;
 
-void processCanRxMessage(const CANRxFrame& frame, Logging* logger) {
+static CanSensorBase* s_head = nullptr;
+
+void serviceCanSubscribers(const CANRxFrame& frame, efitick_t nowNt) {
+	CanSensorBase* current = s_head;
+
+	while (current) {
+		current = current->processFrame(frame, nowNt);
+	}
+}
+
+void registerCanSensor(CanSensorBase& sensor) {
+	sensor.setNext(s_head);
+	s_head = &sensor;
+}
+
+void processCanRxMessage(const CANRxFrame& frame, Logging* logger, efitick_t nowNt) {
 	if (CONFIG(debugMode) == DBG_CAN) {
 		printPacket(frame, logger);
 	}
 
+	serviceCanSubscribers(frame, nowNt);
+
 	// TODO: if/when we support multiple lambda sensors, sensor N
 	// has address 0x0180 + N where N = [0, 15]
 	if (frame.SID == 0x0180) {

firmware/controllers/sensors/can_sensor.h

@@ -0,0 +1,67 @@
+/**
+ * @file	can_sensor.h
+ *
+ * @date March 31, 2020
+ * @author Matthew Kennedy, (c) 2020
+ */
+
+#pragma once
+
+#include "stored_value_sensor.h"
+#include "scaled_channel.h"
+#include "hal.h"
+
+class CanSensorBase : public StoredValueSensor {
+public:
+	CanSensorBase(uint32_t eid, SensorType type, efitick_t timeout)
+		: StoredValueSensor(type, timeout)
+		, m_eid(eid)
+	{
+	}
+
+	void showInfo(Logging* logger, const char* sensorName) const override;
+
+	CanSensorBase* processFrame(const CANRxFrame& frame, efitick_t nowNt) {
+		if (frame.EID == m_eid) {
+			decodeFrame(frame, nowNt);
+		}
+
+		return m_next;
+	}
+
+	void setNext(CanSensorBase* next) {
+		m_next = next;
+	}
+
+protected:
+	virtual void decodeFrame(const CANRxFrame& frame, efitick_t nowNt) = 0;
+
+private:
+	CanSensorBase* m_next = nullptr;
+	const uint32_t m_eid;
+};
+
+template <typename TStorage, int TScale>
+class CanSensor : public CanSensorBase {
+public:
+	CanSensor(uint32_t eid, uint8_t offset, SensorType type, efitick_t timeout)
+		: CanSensorBase(eid, type, timeout)
+		, m_offset(offset)
+	{
+	}
+
+	void decodeFrame(const CANRxFrame& frame, efitick_t nowNt) override {
+		// Compute the location of our data within the frame
+		const uint8_t* dataLocation = &frame.data8[m_offset];
+
+		// 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);
+	}
+
+private:
+	const uint8_t m_offset;
+};

firmware/controllers/sensors/sensor_info_printing.cpp

@@ -1,3 +1,4 @@
+#include "global.h"
 #include "proxy_sensor.h"
 #include "functional_sensor.h"
 #include "efilib.h"
@@ -16,3 +17,12 @@ void FunctionalSensor::showInfo(Logging* logger, const char* sensorName) const {
 		func->showInfo(logger, m_rawValue);
 	}
 }
+
+#if EFI_CAN_SUPPORT
+#include "can_sensor.h"
+
+void CanSensorBase::showInfo(Logging* logger, const char* sensorName) const {
+	const auto [valid, value] = get();
+	scheduleMsg(logger, "CAN Sensor \"%s\": valid: %d value: %.2f", sensorName, valid, value);
+}
+#endif // EFI_CAN_SUPPORT

firmware/hw_layer/drivers/can/can_hw.cpp

@@ -130,7 +130,7 @@ public:
 			// Process the message
 			canReadCounter++;
 
-			processCanRxMessage(m_buffer, &logger);
+			processCanRxMessage(m_buffer, &logger, getTimeNowNt());
 		}
 	}
 

firmware/init/init.mk

@@ -1,4 +1,6 @@
 
 INIT_SRC_CPP =	$(PROJECT_DIR)/init/sensor/init_sensors.cpp \
 				$(PROJECT_DIR)/init/sensor/init_oil_pressure.cpp \
-				$(PROJECT_DIR)/init/sensor/init_tps.cpp
+				$(PROJECT_DIR)/init/sensor/init_tps.cpp \
+				$(PROJECT_DIR)/init/sensor/init_can_sensors.cpp \
+

firmware/init/sensor/init_can_sensors.cpp

@@ -0,0 +1,28 @@
+/**
+ * @file	init_can_sensors.cpp
+ *
+ * @date March 31, 2020
+ * @author Matthew Kennedy, (c) 2020
+ */
+
+#include "global.h"
+
+#if EFI_CAN_SUPPORT
+#include "can_sensor.h"
+#include "can.h"
+
+CanSensor<int16_t, PACK_MULT_PERCENT> canPedalSensor(
+	CAN_DEFAULT_BASE + CAN_PEDAL_TPS_OFFSET, /*offset =*/ 0,
+	SensorType::AcceleratorPedal, MS2NT(100)
+);
+
+void initCanSensors() {
+#if EFI_CANBUS_SLAVE
+	registerCanSensor(canPedalSensor);
+
+	if (!canPedalSensor.Register()) {
+		firmwareError(CUSTOM_INVALID_TPS_SETTING, "Duplicate registration for pedal sensor");
+	}
+#endif // EFI_CANBUS_SLAVE
+}
+#endif // EFI_CAN_SUPPORT

firmware/init/sensor/init_sensors.cpp

@@ -2,6 +2,7 @@
  * @file init_sensorss.cpp
  */
 
+#include "global.h"
 #include "cli_registry.h"
 #include "init.h"
 #include "sensor.h"
@@ -11,8 +12,13 @@ static void initSensorCli(Logging* logger);
 // Sensor init/config
 void initTps();
 void initOilPressure();
+void initCanSensors();
 
 void initNewSensors(Logging* logger) {
+#if EFI_CAN_SUPPORT
+	initCanSensors();
+#endif
+
 	initTps();
 	initOilPressure();