can vss (#3402)

firmware/controllers/can/can_vss.cpp

@@ -12,54 +12,37 @@
 #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;
 
-
+expected<uint16_t> look_up_can_id(can_vss_nbc_e type) {
-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 */
+			return 0x01F0; /* BMW e46 ABS Message */
-            break;
 		case W202:
-            retCanID = 0x0200; /* W202 C180 ABS signal */
+			return 0x0200; /* W202 C180 ABS signal */
-            break;
 		default:
 			firmwareError(OBD_Vehicle_Speed_SensorB, "Wrong Can DBC selected: %d", type);
-            retCanID = 0xffff;
+			return unexpected;
-            break;
 	}
-    return retCanID;
 }
 
-
 /* 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) {
+float processBMW_e46(const CANRxFrame& frame) {
-    
-    uint16_t tmp;
-    
 	/* left front wheel speed is used here */
-    tmp = ((frame.data8[1] & 0x0f) << 8 );
+	uint16_t tmp = ((frame.data8[1] & 0x0f) << 8 );
 	tmp |= frame.data8[0];
 
-    vssSpeed = tmp / 16;
+	return tmp / 16.0f;
-
-
 }
 
-void processW202(const CANRxFrame& frame) {
+float processW202(const CANRxFrame& frame) {
-
+	uint16_t tmp = (frame.data8[2] << 8);
-    uint16_t tmp;
-    
-    tmp = (frame.data8[2] << 8);
 	tmp |= frame.data8[3];
-    vssSpeed = ((float)tmp) * 0.0625;
+	return tmp * 0.0625;
 }
 
 /* End of specific processing functions */
@@ -71,6 +54,21 @@ void canVssInfo(void) {
 	efiPrintf("CONFIG_enableCanVss is %d", CONFIG(enableCanVss));
 }
 
+expected<float> 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;
@@ -81,32 +79,12 @@ void processCanRxVss(const CANRxFrame& frame, efitick_t nowNt) {
 		return;
 	}
 
-    frameTime = nowNt;
+	if (auto speed = processCanRxVssImpl(frame)) {
-    switch (CONFIG(canVssNbcType)){
+		canSpeed.setValidValue(speed.Value, nowNt);
-        case BMW_e46:
-            processBMW_e46(frame);
-            break;
-        case W202:
-            processW202(frame);
-            break;
-        default:
-            efiPrintf("vss unsupported can option selected %x", CONFIG(canVssNbcType) );
-            break;
-    }
 
 #if EFI_DYNO_VIEW
 		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;
 	}
 }
 
@@ -114,15 +92,14 @@ void initCanVssSupport() {
 	addConsoleAction("canvssinfo", canVssInfo);
 
 	if (CONFIG(enableCanVss)) {
-
+		if (auto canId = look_up_can_id(CONFIG(canVssNbcType))) {
+			filterCanID = canId.Value;
+			canSpeed.Register();
 			isInit = true;
-        filterCanID = look_up_can_id(CONFIG(canVssNbcType));
+		} else {
-
-        if (filterCanID == 0xffff) {
 			isInit = false;
 		}
 	}
-    
 }
 
 void setCanVss(int type) {

firmware/controllers/can/can_vss.h

@@ -7,7 +7,6 @@
 
 #pragma once
 
-float getVehicleCanSpeed(void);
 void initCanVssSupport();
 void setCanVss(int type);
 

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;