#ifdef FEATURE_ENABLE_CAN #include "can.h" MCP_CAN CAN0(GPIO_CS_CAN); void Init_CAN() { if (CAN0.begin(MCP_STDEXT, CAN_500KBPS, MCP_16MHZ) != CAN_OK) MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, DTC_CRITICAL, true); 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 = 0; uint16_t RearWheelSpeed_raw; uint32_t milimeters_to_add = 0; 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(); milimeters_to_add = (RWP_millimeter_per_second * timesincelast) / 1000; } if (lastRecTimestamp != 0) { MaintainDTC(DTC_NO_CAN_SIGNAL, DTC_CRITICAL, (millis() > lastRecTimestamp + 10000 ? true : false)); } return milimeters_to_add; } #endif