From 15c40ffb5bb1bcce8ac77a9710f882c05e937561 Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Tue, 31 Mar 2020 20:21:05 -0700 Subject: [PATCH] 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 --- firmware/controllers/can/can.h | 5 +- firmware/controllers/can/can_rx.cpp | 20 +++++- firmware/controllers/sensors/can_sensor.h | 67 +++++++++++++++++++ .../sensors/sensor_info_printing.cpp | 10 +++ firmware/hw_layer/drivers/can/can_hw.cpp | 2 +- firmware/init/init.mk | 4 +- firmware/init/sensor/init_can_sensors.cpp | 28 ++++++++ firmware/init/sensor/init_sensors.cpp | 6 ++ 8 files changed, 138 insertions(+), 4 deletions(-) create mode 100644 firmware/controllers/sensors/can_sensor.h create mode 100644 firmware/init/sensor/init_can_sensors.cpp diff --git a/firmware/controllers/can/can.h b/firmware/controllers/can/can.h index 2477b49cae..71e46d4761 100644 --- a/firmware/controllers/can/can.h +++ b/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: diff --git a/firmware/controllers/can/can_rx.cpp b/firmware/controllers/can/can_rx.cpp index d5807b91fc..8f4e966f2a 100644 --- a/firmware/controllers/can/can_rx.cpp +++ b/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) { diff --git a/firmware/controllers/sensors/can_sensor.h b/firmware/controllers/sensors/can_sensor.h new file mode 100644 index 0000000000..32395ef95c --- /dev/null +++ b/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 +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*>(dataLocation); + + // Actually do the conversion + float value = *scaler; + setValidValue(value, nowNt); + } + +private: + const uint8_t m_offset; +}; diff --git a/firmware/controllers/sensors/sensor_info_printing.cpp b/firmware/controllers/sensors/sensor_info_printing.cpp index 0db441a14c..fec039c580 100644 --- a/firmware/controllers/sensors/sensor_info_printing.cpp +++ b/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 \ No newline at end of file diff --git a/firmware/hw_layer/drivers/can/can_hw.cpp b/firmware/hw_layer/drivers/can/can_hw.cpp index 24cf0f03b0..af77c3308f 100644 --- a/firmware/hw_layer/drivers/can/can_hw.cpp +++ b/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()); } } diff --git a/firmware/init/init.mk b/firmware/init/init.mk index 84b74474d7..2ed6d0efe7 100644 --- a/firmware/init/init.mk +++ b/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 \ + diff --git a/firmware/init/sensor/init_can_sensors.cpp b/firmware/init/sensor/init_can_sensors.cpp new file mode 100644 index 0000000000..07863c47c4 --- /dev/null +++ b/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 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 diff --git a/firmware/init/sensor/init_sensors.cpp b/firmware/init/sensor/init_sensors.cpp index 3b8f91f3fa..3c4745a0ba 100644 --- a/firmware/init/sensor/init_sensors.cpp +++ b/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();