#include "cmsis_os2.h" // CMSIS-RTOS2 header file #include "CanDataTask.h" #include "fdcan.h" #include "CanDataHandler.h" #include "FreeRTOS.h" #include "CLSAddress.h" #include "firmware.pb.h" #include "cls_device.pb.h" #include "usb.pb.h" #include "version_info.h" #include "ulog.h" #include "BSP_POWER.h" // Define thread flags #define FLAG_FDCAN_RX_FIFO0 (1<<0) #define FLAG_FDCAN_RX_FIFO1 (1<<1) // Memory for the task StaticTask_t CanDataTask_cb; uint32_t CanDataTask_stk[512]; // Attributes for the task osThreadId_t CanDataTask_id; const osThreadAttr_t CanDataTask_attr = { .name = "CanDataTask", .attr_bits = 0U, .cb_mem = &CanDataTask_cb, .cb_size = sizeof(CanDataTask_cb), .stack_mem = CanDataTask_stk, .stack_size = sizeof(CanDataTask_stk), .priority = osPriorityNormal, }; // Memory for the task StaticTask_t CarCanTask_cb; uint32_t CarCanTask_stk[256]; // Attributes for the task osThreadId_t CarCanTask_id; const osThreadAttr_t CarCanTask_attr = { .name = "CarCanTask", .attr_bits = 0U, .cb_mem = &CarCanTask_cb, .cb_size = sizeof(CarCanTask_cb), .stack_mem = CarCanTask_stk, .stack_size = sizeof(CarCanTask_stk), .priority = osPriorityNormal, }; uint32_t dlcDecode(uint32_t dlcCode) { switch(dlcCode) { case FDCAN_DLC_BYTES_0: return 0; case FDCAN_DLC_BYTES_1: return 1; case FDCAN_DLC_BYTES_2: return 2; case FDCAN_DLC_BYTES_3: return 3; case FDCAN_DLC_BYTES_4: return 4; case FDCAN_DLC_BYTES_5: return 5; case FDCAN_DLC_BYTES_6: return 6; case FDCAN_DLC_BYTES_7: return 7; case FDCAN_DLC_BYTES_8: return 8; case FDCAN_DLC_BYTES_12: return 12; case FDCAN_DLC_BYTES_16: return 16; case FDCAN_DLC_BYTES_20: return 20; case FDCAN_DLC_BYTES_24: return 24; case FDCAN_DLC_BYTES_32: return 32; case FDCAN_DLC_BYTES_48: return 48; case FDCAN_DLC_BYTES_64: return 64; default: return 0; // Return 0 for unknown dlc } } void CanDataTask_func(void *argument); void CarCanTask_func(void *argument); void CanDataTask_start() { // Task functionality here CanDataTask_id = osThreadNew(CanDataTask_func, NULL, &CanDataTask_attr); CarCanTask_id = osThreadNew(CarCanTask_func, NULL, &CarCanTask_attr); } // Function for the task void CanDataTask_func(void *argument) { /* Configure global filter on FDCAN instanc: Filter all remote frames with STD and EXT ID Reject non matching frames with STD ID and EXT ID */ if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE) != HAL_OK) { Error_Handler(); } /* Start the FDCAN module */ if (HAL_FDCAN_Start(&hfdcan1) != HAL_OK){ Error_Handler(); } if(HAL_FDCAN_ActivateNotification(&hfdcan1,FDCAN_IT_RX_FIFO0_NEW_MESSAGE | FDCAN_IT_RX_FIFO1_NEW_MESSAGE, 0) != HAL_OK) { Error_Handler(); } // setup listening for heartbeats for (size_t i = 0; i < 16; i++) { CanData_regDataMsg(GENERATE_CLS_ADDRESS(CLS_CODE_STATUS, i, CLS_CH_STA_HEATBEAT)); } FDCAN_RxHeaderTypeDef RxHeader; uint8_t RxData[8]; for(;;) { // wait for interrupt event on any fifo osThreadFlagsWait(FLAG_FDCAN_RX_FIFO0 | FLAG_FDCAN_RX_FIFO1, osFlagsWaitAny, osWaitForever); // check the fifos for data and handle it if nessessay while (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO0) > 0 ) { if (HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK) { Error_Handler(); } else { CanData_canFifo0RxCallback(RxHeader.Identifier,RxData, dlcDecode(RxHeader.DataLength)); } } while (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan1, FDCAN_RX_FIFO1) > 0 ) { if (HAL_FDCAN_GetRxMessage(&hfdcan1, FDCAN_RX_FIFO1, &RxHeader, RxData) != HAL_OK) { Error_Handler(); } else { CanData_canFifo1RxCallback(RxHeader.Identifier,RxData, dlcDecode(RxHeader.DataLength)); } } } } static uint64_t last_car_message_time = UINT64_MAX; // convert byte to 2 hex characters void byteToHex(uint8_t byte, char * hex) { const char hexLookup[] = "0123456789ABCDEF"; hex[0] = hexLookup[byte >> 4]; hex[1] = hexLookup[byte & 0x0F]; } void CarCanTask_func(void *argument) { // for testing accept all messages from the car can bus // put unkown messages in fifo 0 if (HAL_FDCAN_ConfigGlobalFilter(&hfdcan2, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_ACCEPT_IN_RX_FIFO0, FDCAN_REJECT_REMOTE, FDCAN_REJECT_REMOTE) != HAL_OK) { Error_Handler(); } FDCAN_FilterTypeDef sFilterConfig; sFilterConfig.IdType = FDCAN_STANDARD_ID; sFilterConfig.FilterIndex = 0; sFilterConfig.FilterType = CLS_BSP_CAN_FILTER_LIST; sFilterConfig.FilterConfig = FDCAN_FILTER_TO_RXFIFO1; sFilterConfig.FilterID1 = 0x391; sFilterConfig.FilterID2 = 0x395; HAL_FDCAN_ConfigFilter(&hfdcan2, &sFilterConfig); /* Start the FDCAN module */ if (HAL_FDCAN_Start(&hfdcan2) != HAL_OK){ Error_Handler(); } if(HAL_FDCAN_ActivateNotification(&hfdcan2,FDCAN_IT_RX_FIFO0_NEW_MESSAGE | FDCAN_IT_RX_FIFO1_NEW_MESSAGE, 0) != HAL_OK) { Error_Handler(); } FDCAN_RxHeaderTypeDef RxHeader; uint8_t RxData[8]; for(;;) { // wait for interrupt event on any fifo osThreadFlagsWait(FLAG_FDCAN_RX_FIFO0 | FLAG_FDCAN_RX_FIFO1, osFlagsWaitAny, osWaitForever); // check the fifos for data and handle it if nessessay while (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan2, FDCAN_RX_FIFO0) > 0 ) { if (HAL_FDCAN_GetRxMessage(&hfdcan2, FDCAN_RX_FIFO0, &RxHeader, RxData) != HAL_OK) { Error_Handler(); } else { // do something with the can data //last_car_message_time = osKernelGetTickCount(); char msg[17] = {0}; switch (RxHeader.DataLength) { case FDCAN_DLC_BYTES_1: byteToHex(RxData[0], &msg[0]); break; case FDCAN_DLC_BYTES_2: byteToHex(RxData[0], &msg[0]); byteToHex(RxData[1], &msg[2]); break; case FDCAN_DLC_BYTES_3: byteToHex(RxData[0], &msg[0]); byteToHex(RxData[1], &msg[2]); byteToHex(RxData[2], &msg[4]); break; case FDCAN_DLC_BYTES_4: byteToHex(RxData[0], &msg[0]); byteToHex(RxData[1], &msg[2]); byteToHex(RxData[2], &msg[4]); byteToHex(RxData[3], &msg[6]); break; case FDCAN_DLC_BYTES_5: byteToHex(RxData[0], &msg[0]); byteToHex(RxData[1], &msg[2]); byteToHex(RxData[2], &msg[4]); byteToHex(RxData[3], &msg[6]); byteToHex(RxData[4], &msg[8]); break; case FDCAN_DLC_BYTES_6: byteToHex(RxData[0], &msg[0]); byteToHex(RxData[1], &msg[2]); byteToHex(RxData[2], &msg[4]); byteToHex(RxData[3], &msg[6]); byteToHex(RxData[4], &msg[8]); byteToHex(RxData[5], &msg[10]); break; case FDCAN_DLC_BYTES_7: byteToHex(RxData[0], &msg[0]); byteToHex(RxData[1], &msg[2]); byteToHex(RxData[2], &msg[4]); byteToHex(RxData[3], &msg[6]); byteToHex(RxData[4], &msg[8]); byteToHex(RxData[5], &msg[10]); byteToHex(RxData[6], &msg[12]); break; case FDCAN_DLC_BYTES_8: byteToHex(RxData[0], &msg[0]); byteToHex(RxData[1], &msg[2]); byteToHex(RxData[2], &msg[4]); byteToHex(RxData[3], &msg[6]); byteToHex(RxData[4], &msg[8]); byteToHex(RxData[5], &msg[10]); byteToHex(RxData[6], &msg[12]); byteToHex(RxData[7], &msg[14]); break; case FDCAN_DLC_BYTES_0: default: /* nothing to do */ break; } ULOG_DEBUG("Car MSG: %x, %d %s", RxHeader.Identifier,CLS_BSP_DLC_ToBytes(RxHeader.DataLength) , msg); } } while (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan2, FDCAN_RX_FIFO1) > 0 ) { if (HAL_FDCAN_GetRxMessage(&hfdcan2, FDCAN_RX_FIFO1, &RxHeader, RxData) != HAL_OK) { Error_Handler(); } else { // do something with the can data ULOG_DEBUG("Car LOCK MSG: %x, %d", RxHeader.Identifier, CLS_BSP_DLC_ToBytes(RxHeader.DataLength)); if(RxHeader.Identifier == 0x391) { if (RxData[1] == 4) { // car was unlocked last_car_message_time = osKernelGetTickCount(); } if (RxData[1] == 80) { // car was locked BSP_POWER_EnterStandby(); } } if (RxHeader.Identifier == 0x395) { // send the unlock message to the car if ((RxData[0] & 0x0F) == 0x01) { // car was unlocked last_car_message_time = osKernelGetTickCount(); } if ((RxData[0] & 0x0F) == 0x02) { // car was locked BSP_POWER_EnterStandby(); } } } } } } /** * @brief Check if a car can message has been received * * @return true if a car can message has been received * @return false if no car can message has been received */ bool CanDataTask_gotCarCanMessage() { return last_car_message_time != UINT64_MAX; } void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) { // Notify the thread if(hfdcan == &hfdcan1) { osThreadFlagsSet(CanDataTask_id, FLAG_FDCAN_RX_FIFO0); } if(hfdcan == &hfdcan2) { osThreadFlagsSet(CarCanTask_id, FLAG_FDCAN_RX_FIFO0); } } void HAL_FDCAN_RxFifo1Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo1ITs) { // Notify the thread if(hfdcan == &hfdcan1) { osThreadFlagsSet(CanDataTask_id, FLAG_FDCAN_RX_FIFO1); } if(hfdcan == &hfdcan2) { osThreadFlagsSet(CarCanTask_id, FLAG_FDCAN_RX_FIFO1); } } void DataClbk_cls_device_ResponseList(void* msg, uint32_t length) { // igored } #include "UsbDataHandler.h" cls_device_ResponseList list; extern uint8_t gCLS_DEVICE_ADDRESS; void DataClbk_cls_device_RequestList(void* msg, uint32_t length) { memset(&list,0,sizeof(list)); // add yourself CLS_Position_t position = CLS_BSP_GetPosition(); CLS_Type_t type = CLS_BSP_GetDeviceType(); list.devices[list.devices_count].available = true; list.devices[list.devices_count].canid = GENERATE_CLS_ADDRESS(CLS_CODE_STATUS, gCLS_DEVICE_ADDRESS, CLS_CH_STA_HEATBEAT); list.devices[list.devices_count].device = gCLS_DEVICE_ADDRESS; list.devices[list.devices_count].type = (uint32_t) type; // enum to uint list.devices[list.devices_count].position[0] = position.p0; list.devices[list.devices_count].position[1] = position.p1; list.devices[list.devices_count].position_count = 2; list.devices[list.devices_count].fw_version[0] = VERSION_INFO.count; list.devices[list.devices_count].fw_version[1] = VERSION_INFO.patch; list.devices[list.devices_count].fw_version[2] = VERSION_INFO.minor; list.devices[list.devices_count].fw_version[3] = VERSION_INFO.major; list.devices_count++; for (size_t i = 0; i < 16; i++) { uint16_t canid = (GENERATE_CLS_ADDRESS(CLS_CODE_STATUS, i, CLS_CH_STA_HEATBEAT)); const CanDataMessage * msg =CanData_getDataMessage(canid); if(msg) { if(msg->data_length > 0) { CLS_HeatbeatData_t data = {0}; memcpy(&data, msg->data, msg->data_length); list.devices[list.devices_count].available = true; list.devices[list.devices_count].canid = canid; list.devices[list.devices_count].device = i; list.devices[list.devices_count].counter = data.counter; list.devices[list.devices_count].type = (uint32_t) data.type; // enum to uint list.devices[list.devices_count].position[0] = data.position.p0; list.devices[list.devices_count].position[1] = data.position.p1; list.devices[list.devices_count].position_count = 2; list.devices[list.devices_count].fw_version[0] = data.firmware_version.count; list.devices[list.devices_count].fw_version[1] = data.firmware_version.patch; list.devices[list.devices_count].fw_version[2] = data.firmware_version.minor; list.devices[list.devices_count].fw_version[3] = data.firmware_version.major; list.devices_count++; } } } USBDataResonse(&list, cls_device_ResponseList_fields, cls_usb_PackageType_RESPONSE_DEVICE_LIST); } cls_device_UpdateDeviceSettings msg_cls_device_UpdateDeviceSettings; void DataClbk_cls_device_UpdateDeviceSettings(void* msg, uint32_t length) { DATA_CLBK_SETUP(cls_device_UpdateDeviceSettings); cls_device_UpdateDeviceSettings * msgs = &msg_cls_device_UpdateDeviceSettings; if(msgs->position_count >= 2) { CLS_Position_t pos; pos.p0 = msgs->position[0]; pos.p1 = msgs->position[1]; CLS_SendEventChangeTypePostion(msgs->device, msgs->type, pos); } }