From e8d612eef88852a8bcfb4c47e5dcdf64a1e5ffc4 Mon Sep 17 00:00:00 2001 From: Matthew Kennedy Date: Sun, 24 Oct 2021 16:04:47 -0700 Subject: [PATCH] can vss (#3402) --- firmware/controllers/can/can_vss.cpp | 143 ++++++++---------- firmware/controllers/can/can_vss.h | 1 - .../controllers/sensors/stored_value_sensor.h | 3 +- 3 files changed, 62 insertions(+), 85 deletions(-) diff --git a/firmware/controllers/can/can_vss.cpp b/firmware/controllers/can/can_vss.cpp index 8b243f1def..e40380b0c6 100644 --- a/firmware/controllers/can/can_vss.cpp +++ b/firmware/controllers/can/can_vss.cpp @@ -12,117 +12,94 @@ #if EFI_CAN_SUPPORT #include "can.h" #include "dynoview.h" +#include "stored_value_sensor.h" static bool isInit = false; static uint16_t filterCanID = 0; -static efitick_t frameTime; -static float vssSpeed = 0; - -uint16_t look_up_can_id(can_vss_nbc_e type) { - - uint16_t retCanID; - switch (type) { - case BMW_e46: - retCanID = 0x01F0; /* BMW e46 ABS Message */ - break; - case W202: - retCanID = 0x0200; /* W202 C180 ABS signal */ - break; - default: - firmwareError(OBD_Vehicle_Speed_SensorB, "Wrong Can DBC selected: %d", type); - retCanID = 0xffff; - break; - } - return retCanID; +expected look_up_can_id(can_vss_nbc_e type) { + switch (type) { + case BMW_e46: + return 0x01F0; /* BMW e46 ABS Message */ + case W202: + return 0x0200; /* W202 C180 ABS signal */ + default: + firmwareError(OBD_Vehicle_Speed_SensorB, "Wrong Can DBC selected: %d", type); + return unexpected; + } } - /* Module specitifc processing functions */ /* source: http://z4evconversion.blogspot.com/2016/07/completely-forgot-but-it-does-live-on.html */ -void processBMW_e46(const CANRxFrame& frame) { - - uint16_t tmp; - - /* left front wheel speed is used here */ - tmp = ((frame.data8[1] & 0x0f) << 8 ); - tmp |= frame.data8[0]; - - vssSpeed = tmp / 16; - +float processBMW_e46(const CANRxFrame& frame) { + /* left front wheel speed is used here */ + uint16_t tmp = ((frame.data8[1] & 0x0f) << 8 ); + tmp |= frame.data8[0]; + return tmp / 16.0f; } -void processW202(const CANRxFrame& frame) { - - uint16_t tmp; - - tmp = (frame.data8[2] << 8); - tmp |= frame.data8[3]; - vssSpeed = ((float)tmp) * 0.0625; +float processW202(const CANRxFrame& frame) { + uint16_t tmp = (frame.data8[2] << 8); + tmp |= frame.data8[3]; + return tmp * 0.0625; } /* End of specific processing functions */ void canVssInfo(void) { - efiPrintf("vss using can option selected %x", CONFIG(canVssNbcType)); - efiPrintf("vss filter for %x canID", filterCanID); - efiPrintf("Vss module is %d", isInit); - efiPrintf("CONFIG_enableCanVss is %d", CONFIG(enableCanVss)); + efiPrintf("vss using can option selected %x", CONFIG(canVssNbcType)); + efiPrintf("vss filter for %x canID", filterCanID); + efiPrintf("Vss module is %d", isInit); + efiPrintf("CONFIG_enableCanVss is %d", CONFIG(enableCanVss)); } +expected processCanRxVssImpl(const CANRxFrame& frame) { + switch (CONFIG(canVssNbcType)){ + case BMW_e46: + return processBMW_e46(frame); + case W202: + return processW202(frame); + default: + efiPrintf("vss unsupported can option selected %x", CONFIG(canVssNbcType) ); + } + + return unexpected; +} + +static StoredValueSensor canSpeed(SensorType::VehicleSpeed, MS2NT(500)); + void processCanRxVss(const CANRxFrame& frame, efitick_t nowNt) { - if ((!CONFIG(enableCanVss)) || (!isInit)) { - return; - } + if ((!CONFIG(enableCanVss)) || (!isInit)) { + return; + } - //filter it we need to process the can message or not - if (CAN_SID(frame) != filterCanID ) { - return; - } + //filter it we need to process the can message or not + if (CAN_SID(frame) != filterCanID ) { + return; + } - frameTime = nowNt; - switch (CONFIG(canVssNbcType)){ - case BMW_e46: - processBMW_e46(frame); - break; - case W202: - processW202(frame); - break; - default: - efiPrintf("vss unsupported can option selected %x", CONFIG(canVssNbcType) ); - break; - } + if (auto speed = processCanRxVssImpl(frame)) { + canSpeed.setValidValue(speed.Value, nowNt); #if EFI_DYNO_VIEW - updateDynoViewCan(PASS_ENGINE_PARAMETER_SIGNATURE); + updateDynoViewCan(PASS_ENGINE_PARAMETER_SIGNATURE); #endif -} - -float getVehicleCanSpeed(void) { - - efitick_t nowNt = getTimeNowNt(); - - if ((nowNt - frameTime ) > NT_PER_SECOND) { - return 0; /* can timeout? */ - } else { - return vssSpeed; - } + } } void initCanVssSupport() { - addConsoleAction("canvssinfo", canVssInfo); + addConsoleAction("canvssinfo", canVssInfo); - if (CONFIG(enableCanVss)) { - - isInit = true; - filterCanID = look_up_can_id(CONFIG(canVssNbcType)); - - if (filterCanID == 0xffff) { - isInit = false; - } - } - + if (CONFIG(enableCanVss)) { + if (auto canId = look_up_can_id(CONFIG(canVssNbcType))) { + filterCanID = canId.Value; + canSpeed.Register(); + isInit = true; + } else { + isInit = false; + } + } } void setCanVss(int type) { diff --git a/firmware/controllers/can/can_vss.h b/firmware/controllers/can/can_vss.h index 4db808b8e9..e3f9e0455a 100644 --- a/firmware/controllers/can/can_vss.h +++ b/firmware/controllers/can/can_vss.h @@ -7,7 +7,6 @@ #pragma once -float getVehicleCanSpeed(void); void initCanVssSupport(); void setCanVss(int type); diff --git a/firmware/controllers/sensors/stored_value_sensor.h b/firmware/controllers/sensors/stored_value_sensor.h index 8d4a7c7582..868e52c52c 100644 --- a/firmware/controllers/sensors/stored_value_sensor.h +++ b/firmware/controllers/sensors/stored_value_sensor.h @@ -51,7 +51,6 @@ public: return value; } -protected: StoredValueSensor(SensorType type, efitick_t timeoutNt) : Sensor(type) , m_timeoutPeriod(timeoutNt) @@ -71,6 +70,8 @@ protected: m_lastUpdate = timestamp; } + void showInfo(const char*) const override { } + private: bool m_isValid = false; float m_value = 0.0f;