282 lines
9.7 KiB
C
282 lines
9.7 KiB
C
#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);
|
|
}
|
|
} |