can vss (#3402)

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;
 
-
+expected<uint16_t> look_up_can_id(can_vss_nbc_e type) {
-uint16_t look_up_can_id(can_vss_nbc_e type) {
+	switch (type) {
-    
+		case BMW_e46:
-    uint16_t retCanID;
+			return 0x01F0; /* BMW e46 ABS Message */
-    switch (type) {
+		case W202:
-        case BMW_e46:
+			return 0x0200; /* W202 C180 ABS signal */
-            retCanID = 0x01F0; /* BMW e46 ABS Message */
+		default:
-            break;
+			firmwareError(OBD_Vehicle_Speed_SensorB, "Wrong Can DBC selected: %d", type);
-        case W202:
+			return unexpected;
-            retCanID = 0x0200; /* W202 C180 ABS signal */
+	}
-            break;
-        default:
-            firmwareError(OBD_Vehicle_Speed_SensorB, "Wrong Can DBC selected: %d", type);
-            retCanID = 0xffff;
-            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) {
-    
+	/* left front wheel speed is used here */
-    uint16_t tmp;
+	uint16_t tmp = ((frame.data8[1] & 0x0f) << 8 );
-    
+	tmp |= frame.data8[0];
-    /* left front wheel speed is used here */
-    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[3];
-    
+	return tmp * 0.0625;
-    tmp = (frame.data8[2] << 8);
-    tmp |= frame.data8[3];
-    vssSpeed = ((float)tmp) * 0.0625;
 }
 
 /* End of specific processing functions */
 
 void canVssInfo(void) {
-    efiPrintf("vss using can option selected %x", CONFIG(canVssNbcType));
+	efiPrintf("vss using can option selected %x", CONFIG(canVssNbcType));
-    efiPrintf("vss filter for %x canID", filterCanID);
+	efiPrintf("vss filter for %x canID", filterCanID);
-    efiPrintf("Vss module is %d", isInit);
+	efiPrintf("Vss module is %d", isInit);
-    efiPrintf("CONFIG_enableCanVss is %d", CONFIG(enableCanVss));
+	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)) {
+	if ((!CONFIG(enableCanVss)) || (!isInit)) {
-        return;
+		return;
-    }
+	}
 
-    //filter it we need to process the can message or not
+	//filter it we need to process the can message or not
-    if (CAN_SID(frame) != filterCanID ) {
+	if (CAN_SID(frame) != filterCanID ) {
-        return;
+		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);
+		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)) {
+	if (CONFIG(enableCanVss)) {
-
+		if (auto canId = look_up_can_id(CONFIG(canVssNbcType))) {
-        isInit = true;
+			filterCanID = canId.Value;
-        filterCanID = look_up_can_id(CONFIG(canVssNbcType));
+			canSpeed.Register();
-
+			isInit = true;
-        if (filterCanID == 0xffff) {
+		} else {
-            isInit = false;
+			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;