oliver/cls_master
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity
Files
804c4c8c696fbdb3fc842af3d0ade76ac617660c
cls_master/Application/Tasks/CanDataTask.c

430 lines
15 KiB
C
Raw Blame History

#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"
#include "BSP_GPIO.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[512];
// 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_unlock_message_time = UINT64_MAX;
static uint64_t last_car_message_time = 0;
// 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 {
char msg[17] = {0};
// do something with the can data
if(RxHeader.Identifier == 0x391) {
if (RxData[1] == 4)
{
// car was unlocked
last_unlock_message_time = osKernelGetTickCount();
byteToHex(RxData[0], &msg[0]);
byteToHex(RxData[1], &msg[2]);
byteToHex(RxData[2], &msg[4]);
}
if (RxData[1] ==0x80)
{
// car was locked
if (!BSP_GPIO_K15isSet()) {
NVIC_SystemReset();
}
byteToHex(RxData[0], &msg[0]);
byteToHex(RxData[1], &msg[2]);
byteToHex(RxData[2], &msg[4]);
}
}
if (RxHeader.Identifier == 0x395) {
// send the unlock message to the car
if ((RxData[0] & 0x0F) == 0x01) {
// car was unlocked
byteToHex(RxData[0], &msg[0]);
last_unlock_message_time = osKernelGetTickCount();
}
if ((RxData[0] & 0x0F) == 0x02) {
// car was locked
if (!BSP_GPIO_K15isSet()) {
NVIC_SystemReset();
}
byteToHex(RxData[0], &msg[0]);
}
}
ULOG_DEBUG("Car LOCK MSG: %x, %d %s", RxHeader.Identifier, CLS_BSP_DLC_ToBytes(RxHeader.DataLength), msg );
}
}
}
}
/**
* @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_unlock_message_time != UINT64_MAX;
}
bool CanDataTask_CarCanActive() {
if (last_car_message_time == 0) {
return false;
}
return osKernelGetTickCount() - last_car_message_time < 1000;
}
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) {
last_car_message_time = osKernelGetTickCount();
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) {
last_car_message_time = osKernelGetTickCount();
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);
}
}
Reference in New Issue View Git Blame Copy Permalink