470 lines
16 KiB
C
470 lines
16 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"
|
|
#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;
|
|
static uint8_t car_can_brightness = 255;
|
|
static float car_can_speed = 0;
|
|
static int car_can_direction = 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);
|
|
|
|
sFilterConfig.FilterIndex = 1;
|
|
sFilterConfig.FilterID1 = 0x351;
|
|
sFilterConfig.FilterID2 = 0x635;
|
|
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) {
|
|
byteToHex(RxData[0], &msg[0]);
|
|
byteToHex(RxData[1], &msg[2]);
|
|
byteToHex(RxData[2], &msg[4]);
|
|
if (RxData[1] == 0x04)
|
|
{
|
|
// car was unlocked
|
|
last_unlock_message_time = osKernelGetTickCount();
|
|
}
|
|
else if (RxData[1] ==0x80)
|
|
{
|
|
// car was locked
|
|
if (!BSP_GPIO_K15isSet()) {
|
|
NVIC_SystemReset();
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
if (RxHeader.Identifier == 0x395) {
|
|
byteToHex(RxData[0], &msg[0]);
|
|
|
|
// send the unlock message to the car
|
|
if ((RxData[0] & 0x0F) == 0x01) {
|
|
// car was unlocked
|
|
last_unlock_message_time = osKernelGetTickCount();
|
|
ULOG_DEBUG("Unlock message received");
|
|
|
|
} else if ((RxData[0] & 0x0F) == 0x02) {
|
|
// car was locked
|
|
if (!BSP_GPIO_K15isSet()) {
|
|
NVIC_SystemReset();
|
|
}
|
|
}
|
|
|
|
}
|
|
|
|
// speed signal
|
|
// AA BB XX YY 00 00 00 00
|
|
// Speed (XX*(2^8)+(YY-1))/190
|
|
// direction AA = 0x00 forward, 0x02 backward
|
|
if (RxHeader.Identifier == 0x351) {
|
|
|
|
uint16_t speed = (RxData[2] << 8) + RxData[3];
|
|
float speed_kmh = (speed - 1) / 190.0;
|
|
car_can_speed = speed_kmh;
|
|
car_can_direction = RxData[0];
|
|
ULOG_DEBUG("Speed: %f, Direction: %d", car_can_speed, car_can_direction);
|
|
|
|
|
|
}
|
|
|
|
// brightness knob in 0 - 100
|
|
if (RxHeader.Identifier == 0x635) {
|
|
// scale the brightness to 0 - 255 only using integer math
|
|
car_can_brightness = ((uint32_t)RxData[0] * 255) / 100;
|
|
ULOG_DEBUG("Brightness: %d", car_can_brightness);
|
|
|
|
}
|
|
|
|
//ULOG_DEBUG("Used Car 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;
|
|
|
|
}
|
|
|
|
uint8_t CanDataTask_CarCanBrightness() {
|
|
return car_can_brightness;
|
|
}
|
|
|
|
float CanDataTask_CarCanSpeed() {
|
|
return car_can_speed;
|
|
}
|
|
|
|
int CanDataTask_CarCanDirectionIsForward() {
|
|
return car_can_direction == 0;
|
|
}
|
|
|
|
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();
|
|
|
|
while (HAL_FDCAN_GetRxFifoFillLevel(&hfdcan2, FDCAN_RX_FIFO0) > 0 ) {
|
|
FDCAN_RxHeaderTypeDef RxHeader;
|
|
uint8_t RxData[8];
|
|
HAL_FDCAN_GetRxMessage(&hfdcan2, FDCAN_RX_FIFO0, &RxHeader, RxData);
|
|
//ignore the message for now
|
|
}
|
|
//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);
|
|
}
|
|
} |