2022-05-04 23:06:15 +02:00
|
|
|
#ifdef FEATURE_ENABLE_CAN
|
2022-02-04 21:28:49 +01:00
|
|
|
#include "can.h"
|
|
|
|
|
|
|
|
MCP_CAN CAN0(GPIO_CS_CAN);
|
|
|
|
|
|
|
|
void Init_CAN()
|
|
|
|
{
|
|
|
|
|
2022-03-10 19:44:43 +01:00
|
|
|
if (CAN0.begin(MCP_STDEXT, CAN_500KBPS, MCP_16MHZ) != CAN_OK)
|
2022-08-22 14:13:55 +02:00
|
|
|
MaintainDTC(DTC_CAN_TRANSCEIVER_FAILED, DTC_CRITICAL, true);
|
2022-02-04 21:28:49 +01:00
|
|
|
|
|
|
|
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()
|
|
|
|
{
|
2022-02-04 23:34:51 +01:00
|
|
|
#define FACTOR_RWP_KMH_890ADV 18 // Divider to convert Raw Data to km/h
|
2022-02-04 21:28:49 +01:00
|
|
|
|
|
|
|
can_frame canMsg;
|
2022-03-09 20:25:02 +01:00
|
|
|
static uint32_t lastRecTimestamp = 0;
|
2022-02-04 21:28:49 +01:00
|
|
|
uint16_t RearWheelSpeed_raw;
|
2023-02-20 09:18:14 +01:00
|
|
|
uint32_t milimeters_to_add = 0;
|
2022-02-04 21:28:49 +01:00
|
|
|
|
|
|
|
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
|
2022-02-04 23:34:51 +01:00
|
|
|
uint32_t RWP_millimeter_per_second = (((uint32_t)RearWheelSpeed_raw * 1000000) / FACTOR_RWP_KMH_890ADV) / 3600;
|
2022-02-04 21:28:49 +01:00
|
|
|
|
|
|
|
uint32_t timesincelast = millis() - lastRecTimestamp;
|
|
|
|
lastRecTimestamp = millis();
|
|
|
|
|
2023-02-20 09:18:14 +01:00
|
|
|
milimeters_to_add = (RWP_millimeter_per_second * timesincelast) / 1000;
|
2022-02-04 21:28:49 +01:00
|
|
|
}
|
2022-03-08 21:23:06 +01:00
|
|
|
|
2023-02-20 09:18:14 +01:00
|
|
|
if (lastRecTimestamp != 0)
|
|
|
|
{
|
|
|
|
MaintainDTC(DTC_NO_CAN_SIGNAL, DTC_CRITICAL, (millis() > lastRecTimestamp + 10000 ? true : false));
|
|
|
|
}
|
2022-03-08 21:23:06 +01:00
|
|
|
|
2023-02-20 09:18:14 +01:00
|
|
|
return milimeters_to_add;
|
2022-02-04 21:28:49 +01:00
|
|
|
}
|
2022-05-04 23:06:15 +02:00
|
|
|
#endif
|