CAN ist working

Software/ChainLube/src/can.cpp

@@ -0,0 +1,43 @@
+#include "can.h"
+
+MCP_CAN CAN0(GPIO_CS_CAN);
+
+void Init_CAN()
+{
+
+    if (CAN0.begin(MCP_STDEXT, CAN_500KBPS, MCP_16MHZ) == CAN_OK)
+        Serial.println("MCP2515 Initialized Successfully!");
+    else
+        Serial.println("Error Initializing MCP2515...");
+
+    CAN0.init_Mask(0, 0, 0x07FF0000); // Init first mask...
+    CAN0.init_Mask(1, 0, 0x07FF0000); // Init second mask...
+    CAN0.init_Filt(0, 0, 0x012D0000); // Init first filter...
+
+    CAN0.setMode(MCP_NORMAL);
+}
+
+uint32_t Process_CAN_WheelSpeed()
+{
+    #define FACTOR_RWP_KMH_890ADV 18 // Divider to convert Raw Data to km/h
+
+    can_frame canMsg;
+    static uint32_t lastRecTimestamp;
+    uint16_t RearWheelSpeed_raw;
+
+    if (CAN0.readMsgBuf(&canMsg.can_id, &canMsg.can_dlc, canMsg.data) == CAN_OK)
+    {
+        RearWheelSpeed_raw = (uint16_t)canMsg.data[5] << 8 | canMsg.data[6];
+        // raw / FACTOR_RWP_KMH_890ADV -> km/h * 100000 -> cm/h / 3600 -> cm/s
+        // raw * 500 -> cm/s
+        uint32_t RWP_millimeter_per_second = (((uint32_t)RearWheelSpeed_raw * 1000000) / FACTOR_RWP_KMH_890ADV ) / 3600;
+
+        uint32_t timesincelast = millis() - lastRecTimestamp;
+        lastRecTimestamp = millis();
+
+        uint32_t milimeters_to_add = (RWP_millimeter_per_second * timesincelast) / 1000;
+
+        return milimeters_to_add;
+    }
+    return 0;
+}