#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" // 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; void CarCanTask_func(void *argument) { // for testing accept all messages from the car can bus 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(); } /* 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(); } } 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 last_car_message_time = osKernelGetTickCount(); } } } } /** * @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); } }