From 28cc0fb2b72fcfa7a40b9f567f7cab76273dcb29 Mon Sep 17 00:00:00 2001 From: binsir Date: Sun, 19 Sep 2021 16:41:59 +0800 Subject: [PATCH] =?UTF-8?q?1.1=20=E5=8A=A8=E6=80=81=E8=8A=82=E7=82=B9=20?= =?UTF-8?q?=E6=B7=BB=E5=8A=A0=E5=8D=87=E7=BA=A7?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .vscode/settings.json | 4 +- App/freertos_action.c | 173 ++++--- App/freertos_action.h | 1 + BSP/bootloader_interface.c | 2 +- BSP/bootloader_interface.h | 2 +- BSP/bsp_common.c | 26 +- BSP/bsp_common.h | 6 +- BSP/insifhtica_QOAR1271.c | 103 +++- BSP/insifhtica_QOAR1271.h | 2 + Core/Src/can.c | 14 +- Core/Src/freertos.c | 48 +- Core/Src/main.c | 5 +- Core/Src/usart.c | 4 +- MDK-ARM/proximity_uavcan_converter.uvoptx | 61 ++- MDK-ARM/proximity_uavcan_converter.uvprojx | 22 +- .../proximity_uavcan_converter_sct.Bak | 19 + UAVCAN/uavcan.c | 455 +++++++++++------- UAVCAN/uavcan.h | 74 +-- UAVCAN/uavcan_filter.c | 2 + proximity_uavcan_converter.ioc | 2 +- 20 files changed, 684 insertions(+), 341 deletions(-) create mode 100644 MDK-ARM/proximity_uavcan_converter/proximity_uavcan_converter_sct.Bak diff --git a/.vscode/settings.json b/.vscode/settings.json index 932ed00..1155ad1 100644 --- a/.vscode/settings.json +++ b/.vscode/settings.json @@ -2,6 +2,8 @@ "files.associations": { "*.dat": "makefile", "*.c": "c", - "uavcan.h": "c" + "uavcan.h": "c", + "freertos_action.h": "c", + "bsp_common.h": "c" } } \ No newline at end of file diff --git a/App/freertos_action.c b/App/freertos_action.c index 74fdaf2..a00acd1 100644 --- a/App/freertos_action.c +++ b/App/freertos_action.c @@ -1,20 +1,32 @@ #include "freertos_action.h" +extern uint32_t last_rev_usart1_tick; +extern uint32_t last_rev_usart2_tick; + void task_usart_rev_action(void) { usart_data_t data = {0}; // uint8_t data_uart; osStatus_t os_status; - + active_uasrt_irq(1); + active_uasrt_irq(2); for(;;) { + if (usart_rev_queueHandle != NULL) { os_status = osMessageQueueGet(usart_rev_queueHandle, &data, NULL, osWaitForever); if (os_status == osOK) - { - parse_ar1271_data(&data); + { + if (data.len < 6) + { + deal_send_data(&data); + } + else + { + parse_ar1271_data(&data); + } } } else @@ -55,6 +67,7 @@ void task_uavcan_send_action(void) for(;;) { + allocation_update(); if (uavcan_send_queueHandle != NULL) { status = osMessageQueueGet(uavcan_send_queueHandle, &data, NULL, 5); @@ -63,14 +76,15 @@ void task_uavcan_send_action(void) send_uavcan_data(&data); } } - send_canard(); + send_canard(); + check_need_to_jump_boot(); } } void task_default_action(void) { - //init_allocation(); - uavcan_init_with_cubemx(); + + uavcan_init_by_allocation(); uint32_t last_send_node_status_tick = 0; //uint32_t last_send_proximity_tick =0; /* Infinite loop */ @@ -95,64 +109,106 @@ void task_default_action(void) void task_usart_send(void) { - osDelay(1000); - request_QOAR1271_data(); - active_uasrt_irq(1); - active_uasrt_irq(2); - usart_rev_tick = HAL_GetTick(); + osStatus os_status; + usart_data_t data = {0}; for (;;) { - osDelay(1000); - if ((usart_rev_tick + 120000) < HAL_GetTick()) + if (usart_send_queueHandle != NULL) + { + os_status = osMessageQueueGet(usart_send_queueHandle, &data, NULL, 10); + if (os_status == osOK) + { + if (data.usart_instance == USART_INSTANCE_U1) + { + HAL_UART_Transmit_DMA(&huart1, (uint8_t *)&data.data, data.len); + } + else + { + HAL_UART_Transmit_DMA(&huart2, (uint8_t *)&data.data, data.len); + } + } + } + + if ((last_rev_usart1_tick + 3000) < HAL_GetTick()) { - request_QOAR1271_data(); - usart_rev_tick = HAL_GetTick(); + request_qoar1271_by_step(1, USART_INSTANCE_U1); + } + + if ((last_rev_usart2_tick + 3000) < HAL_GetTick()) + { + request_qoar1271_by_step(1, USART_INSTANCE_U2); } } } void task_proximity(void) { - // proximity_data_t proximity1 ={0}; - // proximity_data_t proximity2 ={0}; - // osStatus_t os_p1 =osError; - // osStatus_t os_p2 =osError; + proximity_data_t proximity1 = {0}; + proximity_data_t proximity2 = {0}; + osStatus_t os_p1 = osError; + osStatus_t os_p2 = osError; for (;;) { - osDelay(10000); -// if (proximity1_queueHandle != NULL && proximity2_queueHandle != NULL) -// { -// os_p1 = osMessageQueueGet(proximity1_queueHandle, &proximity1, NULL, 100); -// os_p2 = osMessageQueueGet(proximity2_queueHandle, &proximity2, NULL, 100); - -// if ((os_p1 == osOK) && (os_p2 == osOK)) -// { -// uavcan_proximity_t data = {0}; -// data.d0 = min(proximity1.d0, proximity2.d0); -// data.d315 = proximity1.d_left; -// data.d45 = proximity2.d_left; -// send_uavcan_proximity(&data); -// } -// else -// { -// if (os_p1 == osOK) //左边的雷达是反的 -// { -// uavcan_proximity_t data = {0}; -// data.d0 = proximity1.d0; -// data.d315 = proximity1.d_right; -// data.d45 = proximity1.d_left; -// send_uavcan_proximity(&data); -// } -// else if (os_p2 == osOK) -// { -// uavcan_proximity_t data = {0}; -// data.d0 = proximity2.d0; -// data.d315 = proximity2.d_left; -// data.d45 = proximity2.d_right; -// send_uavcan_proximity(&data); -// } -// } -// } + if (proximity1_queueHandle != NULL && proximity2_queueHandle != NULL) + { + os_p1 = osMessageQueueGet(proximity1_queueHandle, &proximity1, NULL, 120); + os_p2 = osMessageQueueGet(proximity2_queueHandle, &proximity2, NULL, 120); + + if ((os_p1 == osOK) && (os_p2 == osOK)) + { + uavcan_proximity_t data = {0}; + + data.d135 = 0xFFFF; + data.d90 = 0xFFFF; + data.d180 = 0xFFFF; + data.d225 = 0xFFFF; + data.d270 = 0xFFFF; + data.d0 = 0xFFFF; + data.d315 = 0xFFFF; + data.d45 = 0xFFFF; + data.d0 = min(proximity1.d0, proximity2.d0); + data.d315 = proximity1.d_left; + data.d45 = proximity2.d_left; + send_uavcan_proximity(&data); + } + else + { + if (os_p1 == osOK) //左边的雷达是反的 + { + uavcan_proximity_t data = {0}; + data.d135 = 0xFFFF; + data.d90 = 0xFFFF; + data.d180 = 0xFFFF; + data.d225 = 0xFFFF; + data.d270 = 0xFFFF; + data.d0 = 0xFFFF; + data.d315 = 0xFFFF; + data.d45 = 0xFFFF; + + data.d0 = proximity1.d0; + data.d315 = proximity1.d_right; + data.d45 = proximity1.d_left; + send_uavcan_proximity(&data); + } + else if (os_p2 == osOK) + { + uavcan_proximity_t data = {0}; + data.d135 = 0xFFFF; + data.d90 = 0xFFFF; + data.d180 = 0xFFFF; + data.d225 = 0xFFFF; + data.d270 = 0xFFFF; + data.d0 = 0xFFFF; + data.d315 = 0xFFFF; + data.d45 = 0xFFFF; + + data.d0 = proximity2.d0; + data.d315 = proximity2.d_left; + data.d45 = proximity2.d_right; + send_uavcan_proximity(&data); + } + } + } } } @@ -161,13 +217,4 @@ uint16_t min(uint16_t one, uint16_t two) return one < two ? one : two; } -//void send_uavcan_proximity(uavcan_proximity_t *data) -//{ -// uavcan_send_data_t msg = {0}; -// if (uavcan_send_queueHandle != NULL) -// { -// msg.id = ZRZK_EQUIPMENT_RANGE_SENSOR_PROXIMITY_ID; -// memcpy(&msg.content.proximity, data, sizeof(uavcan_proximity_t)); -// osMessageQueuePut(uavcan_send_queueHandle, &msg, NULL, 10U); -// } -//} + diff --git a/App/freertos_action.h b/App/freertos_action.h index 809aa20..366a200 100644 --- a/App/freertos_action.h +++ b/App/freertos_action.h @@ -14,6 +14,7 @@ extern osMessageQueueId_t uavcan_send_queueHandle; extern osMessageQueueId_t depth_data_queueHandle; extern osMessageQueueId_t proximity1_queueHandle; extern osMessageQueueId_t proximity2_queueHandle; +extern osMessageQueueId_t usart_send_queueHandle; extern osMutexId_t uavcan_send_mutexHandle; extern osMutexId_t radar_parse_mutexHandle; diff --git a/BSP/bootloader_interface.c b/BSP/bootloader_interface.c index fdb105c..9dcacf0 100644 --- a/BSP/bootloader_interface.c +++ b/BSP/bootloader_interface.c @@ -8,6 +8,6 @@ #include "bootloader_interface.h" -struct bootloader_interface bootloader_interface + struct bootloader_interface bootloader_interface __attribute__((at(BOOTLOADER_INTERFACE_ADDR)));// __attribute__((section("MY_RAM"),zero_init));//__attribute__((at( BOOTLOADER_INTERFACE_ADDR))); diff --git a/BSP/bootloader_interface.h b/BSP/bootloader_interface.h index 4a5b269..2add8f4 100644 --- a/BSP/bootloader_interface.h +++ b/BSP/bootloader_interface.h @@ -11,7 +11,7 @@ #include #define BOOTLOADER_INTERFACE_VALID_MAGIC 0x6832F3E0 -#define BOOTLOADER_INTERFACE_ADDR 0x20004E00//0x20004E00 +#define BOOTLOADER_INTERFACE_ADDR 0x20004F00//0x20004E00 struct bootloader_interface { uint32_t magic; diff --git a/BSP/bsp_common.c b/BSP/bsp_common.c index a68f243..94a2a96 100644 --- a/BSP/bsp_common.c +++ b/BSP/bsp_common.c @@ -1,7 +1,7 @@ /* * @Author: your name * @Date: 2021-06-01 17:04:59 - * @LastEditTime: 2021-08-13 15:17:04 + * @LastEditTime: 2021-09-08 10:54:12 * @LastEditors: Please set LastEditors * @Description: In User Settings Edit * @FilePath: \flow-radar-convertor\BSP\bsp_common.c @@ -17,6 +17,9 @@ extern DMA_HandleTypeDef hdma_usart1_tx; uint32_t usart_rev_tick; + +void avtion_do_can_back(CAN_HandleTypeDef *hcan); + void open_timer(TIM_HandleTypeDef *htim) { if (htim == NULL) @@ -37,22 +40,27 @@ void close_timer(TIM_HandleTypeDef *htim) void HAL_CAN_RxFifo1MsgPendingCallback(CAN_HandleTypeDef *hcan) { - uavcan_rev_t can_rev = {0}; + avtion_do_can_back(hcan); +} + +void HAL_CAN_RxFifo0MsgPendingCallback(CAN_HandleTypeDef *hcan) +{ + avtion_do_can_back(hcan); +} + +void avtion_do_can_back(CAN_HandleTypeDef *hcan) +{ + uavcan_rev_t can_rev = {0}; can_rev.tick_ms = HAL_GetTick(); if (canardSTM32Receive(&can_rev.canard_CAN_frame) > 0) { if (uavcan_rev_queueHandle != NULL) { osStatus status = osMessageQueuePut(uavcan_rev_queueHandle, &can_rev, 0U, 0U); - // if(status==osOK) - // { - // } } } } -//-----------usart funtion------- - void active_uasrt_irq(uint8_t instance) { switch (instance) @@ -88,7 +96,7 @@ void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size) usart_data_t msg = {0}; msg.len = Size; - msg.usart_instance = 1; + msg.usart_instance = USART_INSTANCE_U1; if (Size > BUFFER_SIZE) { @@ -110,7 +118,7 @@ void HAL_UARTEx_RxEventCallback(UART_HandleTypeDef *huart, uint16_t Size) usart_data_t msg = {0}; msg.len = Size; - msg.usart_instance = 2; + msg.usart_instance = USART_INSTANCE_U2; if (Size > BUFFER_SIZE) { diff --git a/BSP/bsp_common.h b/BSP/bsp_common.h index b932775..76d2c16 100644 --- a/BSP/bsp_common.h +++ b/BSP/bsp_common.h @@ -1,7 +1,7 @@ /* * @Author: your name * @Date: 2021-06-01 17:05:15 - * @LastEditTime: 2021-08-10 17:10:46 + * @LastEditTime: 2021-09-08 10:53:16 * @LastEditors: Please set LastEditors * @Description: In User Settings Edit * @FilePath: \flow-radar-convertor\BSP\bsp_common.h @@ -14,8 +14,8 @@ extern "C" { #endif #include "main.h" - - +#define USART_INSTANCE_U1 1 +#define USART_INSTANCE_U2 2 #define RANGE_FINDER_ID 13 diff --git a/BSP/insifhtica_QOAR1271.c b/BSP/insifhtica_QOAR1271.c index 599514b..8763644 100644 --- a/BSP/insifhtica_QOAR1271.c +++ b/BSP/insifhtica_QOAR1271.c @@ -1,9 +1,15 @@ #include "insifhtica_QOAR1271.h" #include "uavcan.h" +#include "custom_data_def.h" + uint8_t ar1271_data[LIDAR_360_SIZE]; //static uint8_t data = 0; //static uint8_t step = 0; uint16_t ar1271_rev_flag = 0; + +uint32_t last_rev_usart1_tick; +uint32_t last_rev_usart2_tick; + extern void parse_ar1271_data(usart_data_t *usart_data) { osSemaphoreAcquire(radar_parse_mutexHandle, 30); @@ -33,14 +39,16 @@ extern void parse_ar1271_data(usart_data_t *usart_data) // check if message has right CRC if (crc_crc8(buffer, 19) == buffer[19]) { + usart_rev_tick = HAL_GetTick(); proximity_data_t proximity_data = {0}; proximity_data.d0 = uint16_value(buffer[2], buffer[3]); proximity_data.d_right = uint16_value(buffer[4], buffer[5]); proximity_data.d_left = uint16_value(buffer[16], buffer[17]); - if (usart_data->usart_instance == 1) + if (usart_data->usart_instance == USART_INSTANCE_U1) { + last_rev_usart1_tick = HAL_GetTick(); uavcan_proximity_t data = {0}; // data.d0 = 0xFFFF; // data.d45 = 0xFFFF; @@ -62,8 +70,9 @@ extern void parse_ar1271_data(usart_data_t *usart_data) // osMessageQueuePut(proximity1_queueHandle, &proximity_data, 0, 10); // } } - else if (usart_data->usart_instance == 2) + else if (usart_data->usart_instance == USART_INSTANCE_U2) { + last_rev_usart2_tick = HAL_GetTick(); if (proximity2_queueHandle != NULL) { osMessageQueuePut(proximity2_queueHandle, &proximity_data, 0, 10); @@ -133,3 +142,93 @@ void request_QOAR1271_data(void) HAL_UART_Transmit_DMA(&huart1, send_data4, 5); osDelay(200); } + +void request_qoar1271_by_step(uint8_t step, uint8_t usart_index) +{ + usart_data_t send_data = {0}; + send_data.usart_instance = usart_index; + if(USART_INSTANCE_U1 == usart_index) + { + last_rev_usart1_tick = HAL_GetTick(); + } + else if(USART_INSTANCE_U2 == usart_index) + { + last_rev_usart2_tick = HAL_GetTick(); + } + + switch (step) + { + case 1: + + send_data.len = 4; + send_data.data[0] = 0x00; + send_data.data[1] = 0x11; + send_data.data[2] = 0x02; + send_data.data[3] = 0x4C; + osMessageQueuePut(usart_send_queueHandle, &send_data, NULL, 0); + break; + case 2: + + send_data.len = 4; + send_data.data[0] = 0x00; + send_data.data[1] = 0x31; + send_data.data[2] = 0x03; + send_data.data[3] = 0xE5; + osMessageQueuePut(usart_send_queueHandle, &send_data, NULL, 0); + break; + case 3: + + send_data.len = 5; + send_data.data[0] = 0x00; + send_data.data[1] = 0x52; + send_data.data[2] = 0x02; + send_data.data[3] = 0x01; + send_data.data[4] = 0xDF; + osMessageQueuePut(usart_send_queueHandle, &send_data, NULL, 0); + break; + case 4: + + send_data.len = 5; + send_data.data[0] = 0x00; + send_data.data[1] = 0x52; + send_data.data[2] = 0x03; + send_data.data[3] = 0x03; + send_data.data[4] = 0xC4; + osMessageQueuePut(usart_send_queueHandle, &send_data, NULL, 0); + break; + default: + break; + } +} + +void deal_send_data(usart_data_t *rev_data) +{ + + if ((rev_data->data[0] == 0x00) && + (rev_data->data[1] == 0x11) && + (rev_data->data[2] == 0x02) && + (rev_data->data[3] == 0x4C)) + { + request_qoar1271_by_step(2, rev_data->usart_instance); + } + else if ((rev_data->data[0] == 0x00) && + (rev_data->data[1] == 0x31) && + (rev_data->data[2] == 0x03) && + (rev_data->data[3] == 0xE5)) + { + request_qoar1271_by_step(3, rev_data->usart_instance); + } + else if ((rev_data->data[0] == 0x00) && + (rev_data->data[1] == 0x52) && + (rev_data->data[2] == 0x02) && + (rev_data->data[3] == 0x01) ) + { + request_qoar1271_by_step(4, rev_data->usart_instance); + } + else if ((rev_data->data[0] == 0x00) && + (rev_data->data[1] == 0x52) && + (rev_data->data[2] == 0x03) && + (rev_data->data[3] == 0x03) ) + { + } +} diff --git a/BSP/insifhtica_QOAR1271.h b/BSP/insifhtica_QOAR1271.h index 8376508..eb4ba14 100644 --- a/BSP/insifhtica_QOAR1271.h +++ b/BSP/insifhtica_QOAR1271.h @@ -24,4 +24,6 @@ bool patse_ar1271_data(uint8_t *in_data, uint8_t datalengh, uint8_t *out_data); uint16_t uint16_value(uint8_t h_byte,uint8_t l_byte); void parse_ar1271_data(usart_data_t *usart_data); void request_QOAR1271_data(void); +void request_qoar1271_by_step(uint8_t step, uint8_t usart_index); +void deal_send_data(usart_data_t *rev_data); #endif /* INSIFHTICA_QOAR1271_H */ diff --git a/Core/Src/can.c b/Core/Src/can.c index fcb30ef..46a5745 100644 --- a/Core/Src/can.c +++ b/Core/Src/can.c @@ -129,6 +129,10 @@ void can_init_irq(void) HAL_NVIC_EnableIRQ(CAN1_RX1_IRQn); HAL_CAN_Start(&hcan); HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO1_MSG_PENDING); + + HAL_NVIC_EnableIRQ(USB_LP_CAN1_RX0_IRQn); + HAL_CAN_Start(&hcan); + HAL_CAN_ActivateNotification(&hcan, CAN_IT_RX_FIFO0_MSG_PENDING); } HAL_StatusTypeDef can_filter_init_with_mask(uint32_t bank, uint32_t id, uint32_t mask, uint8_t fifo_index) @@ -159,7 +163,6 @@ HAL_StatusTypeDef can_filter_init_with_mask(uint32_t bank, uint32_t id, uint32_t HAL_StatusTypeDef can_filter_none(void) { - HAL_StatusTypeDef state = HAL_ERROR; CAN_FilterTypeDef filter1; filter1.FilterBank = 1; filter1.FilterMode = CAN_FILTERMODE_IDMASK; @@ -168,10 +171,13 @@ HAL_StatusTypeDef can_filter_none(void) filter1.FilterIdLow = 0; filter1.FilterMaskIdHigh = 0; filter1.FilterMaskIdLow = 0; - filter1.FilterFIFOAssignment = CAN_FILTER_FIFO1; //FIFO0 - state = HAL_CAN_ConfigFilter(&hcan, &filter1); + filter1.FilterFIFOAssignment = CAN_FILTER_FIFO0; //FIFO0 + filter1.FilterActivation = ENABLE; - return state; + if (HAL_CAN_ConfigFilter(&hcan, &filter1) != HAL_OK) + { + Error_Handler(); + } } /* USER CODE END 1 */ diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c index aee6895..73ecd7f 100644 --- a/Core/Src/freertos.c +++ b/Core/Src/freertos.c @@ -26,7 +26,7 @@ /* Private includes ----------------------------------------------------------*/ /* USER CODE BEGIN Includes */ - +#include "freertos_action.h" /* USER CODE END Includes */ /* Private typedef -----------------------------------------------------------*/ @@ -115,6 +115,11 @@ osMessageQueueId_t proximity2_queueHandle; const osMessageQueueAttr_t proximity2_queue_attributes = { .name = "proximity2_queue" }; +/* Definitions for usart_send_queue */ +osMessageQueueId_t usart_send_queueHandle; +const osMessageQueueAttr_t usart_send_queue_attributes = { + .name = "usart_send_queue" +}; /* Definitions for uavcan_send_mutex */ osMutexId_t uavcan_send_mutexHandle; const osMutexAttr_t uavcan_send_mutex_attributes = { @@ -173,16 +178,19 @@ void MX_FREERTOS_Init(void) { uavcan_rev_queueHandle = osMessageQueueNew (10, sizeof(uavcan_rev_t), &uavcan_rev_queue_attributes); /* creation of uavcan_send_queue */ - uavcan_send_queueHandle = osMessageQueueNew (5, sizeof(uavcan_send_data_t), &uavcan_send_queue_attributes); + uavcan_send_queueHandle = osMessageQueueNew (3, sizeof(uavcan_send_data_t), &uavcan_send_queue_attributes); /* creation of usart_rev_queue */ - usart_rev_queueHandle = osMessageQueueNew (10, sizeof(usart_data_t), &usart_rev_queue_attributes); + usart_rev_queueHandle = osMessageQueueNew (4, sizeof(usart_data_t), &usart_rev_queue_attributes); /* creation of proximity1_queue */ - proximity1_queueHandle = osMessageQueueNew (5, sizeof(proximity_data_t), &proximity1_queue_attributes); + proximity1_queueHandle = osMessageQueueNew (2, sizeof(proximity_data_t), &proximity1_queue_attributes); /* creation of proximity2_queue */ - proximity2_queueHandle = osMessageQueueNew (5, sizeof(proximity_data_t), &proximity2_queue_attributes); + proximity2_queueHandle = osMessageQueueNew (2, sizeof(proximity_data_t), &proximity2_queue_attributes); + + /* creation of usart_send_queue */ + usart_send_queueHandle = osMessageQueueNew (8, sizeof(usart_data_t), &usart_send_queue_attributes); /* USER CODE BEGIN RTOS_QUEUES */ /* add queues, ... */ @@ -228,10 +236,7 @@ void StartDefaultTask(void *argument) { /* USER CODE BEGIN StartDefaultTask */ /* Infinite loop */ - for(;;) - { - osDelay(1); - } + task_default_action(); /* USER CODE END StartDefaultTask */ } @@ -246,10 +251,7 @@ void StartUavcanRevTask(void *argument) { /* USER CODE BEGIN StartUavcanRevTask */ /* Infinite loop */ - for(;;) - { - osDelay(1); - } + task_uavcan_rev_action(); /* USER CODE END StartUavcanRevTask */ } @@ -264,10 +266,7 @@ void StartTaskUsartSend(void *argument) { /* USER CODE BEGIN StartTaskUsartSend */ /* Infinite loop */ - for(;;) - { - osDelay(1); - } + task_usart_send(); /* USER CODE END StartTaskUsartSend */ } @@ -282,10 +281,7 @@ void StartUavcanSendTask(void *argument) { /* USER CODE BEGIN StartUavcanSendTask */ /* Infinite loop */ - for(;;) - { - osDelay(1); - } + task_uavcan_send_action(); /* USER CODE END StartUavcanSendTask */ } @@ -300,10 +296,7 @@ void StartUsartRevTask(void *argument) { /* USER CODE BEGIN StartUsartRevTask */ /* Infinite loop */ - for(;;) - { - osDelay(1); - } + task_usart_rev_action(); /* USER CODE END StartUsartRevTask */ } @@ -318,10 +311,7 @@ void StartProximity(void *argument) { /* USER CODE BEGIN StartProximity */ /* Infinite loop */ - for(;;) - { - osDelay(1); - } + task_proximity(); /* USER CODE END StartProximity */ } diff --git a/Core/Src/main.c b/Core/Src/main.c index 89e1a5b..4c347f3 100644 --- a/Core/Src/main.c +++ b/Core/Src/main.c @@ -37,6 +37,7 @@ /* Private define ------------------------------------------------------------*/ /* USER CODE BEGIN PD */ +#define APPLICATION_ADDRESS 0x08004800 /* USER CODE END PD */ /* Private macro -------------------------------------------------------------*/ @@ -69,7 +70,7 @@ void MX_FREERTOS_Init(void); int main(void) { /* USER CODE BEGIN 1 */ - + SCB->VTOR = FLASH_BASE | APPLICATION_ADDRESS; /* USER CODE END 1 */ /* MCU Configuration--------------------------------------------------------*/ @@ -110,7 +111,7 @@ int main(void) while (1) { /* USER CODE END WHILE */ - + /* USER CODE BEGIN 3 */ } /* USER CODE END 3 */ diff --git a/Core/Src/usart.c b/Core/Src/usart.c index 3324073..72bc31f 100644 --- a/Core/Src/usart.c +++ b/Core/Src/usart.c @@ -214,8 +214,8 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) __HAL_LINKDMA(uartHandle,hdmarx,hdma_usart2_rx); /* USART2 interrupt Init */ - // HAL_NVIC_SetPriority(USART2_IRQn, 6, 0); - // HAL_NVIC_EnableIRQ(USART2_IRQn); + HAL_NVIC_SetPriority(USART2_IRQn, 6, 0); + HAL_NVIC_EnableIRQ(USART2_IRQn); /* USER CODE BEGIN USART2_MspInit 1 */ /* USER CODE END USART2_MspInit 1 */ diff --git a/MDK-ARM/proximity_uavcan_converter.uvoptx b/MDK-ARM/proximity_uavcan_converter.uvoptx index 0cedcea..f716509 100644 --- a/MDK-ARM/proximity_uavcan_converter.uvoptx +++ b/MDK-ARM/proximity_uavcan_converter.uvoptx @@ -117,6 +117,26 @@ STLink\ST-LINKIII-KEIL_SWO.dll + + 0 + ARMRTXEVENTFLAGS + -L70 -Z18 -C0 -M0 -T1 + + + 0 + DLGTARM + (1010=-1,-1,-1,-1,0)(1007=-1,-1,-1,-1,0)(1008=-1,-1,-1,-1,0)(1009=-1,-1,-1,-1,0) + + + 0 + ARMDBGFLAGS + + + + 0 + DLGUARM + (105=-1,-1,-1,-1,0) + 0 UL2CM3 @@ -125,17 +145,50 @@ 0 ST-LINKIII-KEIL_SWO - -U-O142 -O2254 -S0 -C0 -N00("ARM CoreSight SW-DP") -D00(2BA01477) -L00(0) -TO18 -TC10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128 -FS08000000 -FL010000 -FP0($$Device:STM32F103T8$Flash\STM32F10x_128.FLM) + -U32FF6E064D48363416550243 -O2254 -SF10000 -C0 -A0 -I0 -HNlocalhost -HP7184 -P1 -N00("ARM CoreSight SW-DP (ARM Core") -D00(1BA01477) -L00(0) -TO131090 -TC10000000 -TT10000000 -TP21 -TDS8007 -TDT0 -TDC1F -TIEFFFFFFFF -TIP8 -FO7 -FD20000000 -FC800 -FN1 -FF0STM32F10x_128.FLM -FS08004800 -FL0B800 -FP0($$Device:STM32F103T8$Flash\STM32F10x_128.FLM) - + + + 0 + 0 + 573 + 1 +
134250792
+ 0 + 0 + 0 + 0 + 0 + 1 + ..\UAVCAN\uavcan.c + + \\proximity_uavcan_converter\../UAVCAN/uavcan.c\573 + + + 1 + 0 + 118 + 1 +
0
+ 0 + 0 + 0 + 0 + 0 + 0 + ..\UAVCAN\uavcan.c + + + + 0 0 1 - 1 + 0 0 0 0 @@ -173,7 +226,7 @@ 1 - 1 + 0 0 2 10000000 diff --git a/MDK-ARM/proximity_uavcan_converter.uvprojx b/MDK-ARM/proximity_uavcan_converter.uvprojx index 99f81c3..575b855 100644 --- a/MDK-ARM/proximity_uavcan_converter.uvprojx +++ b/MDK-ARM/proximity_uavcan_converter.uvprojx @@ -80,9 +80,9 @@ 0 - 0 + 1 0 - + fromelf --bin -o "$Lpriximity.bin" "#L" 0 0 @@ -138,7 +138,7 @@ 1 BIN\UL2V8M.DLL - + "" () @@ -201,7 +201,7 @@ 0 0 0 - 0 + 1 0 0 0 @@ -211,7 +211,7 @@ 0 0 1 - 0 + 1 0 @@ -275,8 +275,8 @@ 1 - 0x8000000 - 0x10000 + 0x8004800 + 0xb800 1 @@ -301,12 +301,12 @@ 0 0x20000000 - 0x5000 + 0x4f00 0 - 0x0 - 0x0 + 0x4f00 + 0x100 @@ -339,7 +339,7 @@ USE_HAL_DRIVER,STM32F103xB - ../Core/Inc;../Drivers/STM32F1xx_HAL_Driver/Inc;../Drivers/STM32F1xx_HAL_Driver/Inc/Legacy;../Middlewares/Third_Party/FreeRTOS/Source/include;../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2;../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM3;../Drivers/CMSIS/Device/ST/STM32F1xx/Include;../Drivers/CMSIS/Include;..\UAVCAN\libcanard\drivers\stm32;..\UAVCAN\libcanard;..\UAVCAN;..\App;..\BSP + ../Core/Inc; ../Drivers/STM32F1xx_HAL_Driver/Inc; ../Drivers/STM32F1xx_HAL_Driver/Inc/Legacy; ../Middlewares/Third_Party/FreeRTOS/Source/include; ../Middlewares/Third_Party/FreeRTOS/Source/CMSIS_RTOS_V2; ../Middlewares/Third_Party/FreeRTOS/Source/portable/RVDS/ARM_CM3; ../Drivers/CMSIS/Device/ST/STM32F1xx/Include; ../Drivers/CMSIS/Include; ..\UAVCAN\libcanard\drivers\stm32; ..\UAVCAN\libcanard; ..\UAVCAN; ..\App; ..\BSP diff --git a/MDK-ARM/proximity_uavcan_converter/proximity_uavcan_converter_sct.Bak b/MDK-ARM/proximity_uavcan_converter/proximity_uavcan_converter_sct.Bak new file mode 100644 index 0000000..beba905 --- /dev/null +++ b/MDK-ARM/proximity_uavcan_converter/proximity_uavcan_converter_sct.Bak @@ -0,0 +1,19 @@ +; ************************************************************* +; *** Scatter-Loading Description File generated by uVision *** +; ************************************************************* + +LR_IROM1 0x08000000 0x00010000 { ; load region size_region + ER_IROM1 0x08000000 0x00010000 { ; load address = execution address + *.o (RESET, +First) + *(InRoot$$Sections) + .ANY (+RO) + .ANY (+XO) + } + RW_IRAM1 0x20000000 0x00004F00 { ; RW data + .ANY (+RW +ZI) + } + RW_IRAM2 0x00004F00 UNINIT 0x00000100 { + .ANY (+RW +ZI) + } +} + diff --git a/UAVCAN/uavcan.c b/UAVCAN/uavcan.c index 8c2a439..767192a 100644 --- a/UAVCAN/uavcan.c +++ b/UAVCAN/uavcan.c @@ -4,8 +4,10 @@ #include "main.h" #include "math.h" #include "can.h" - +#include "stm32f1xx_hal_tim.h" #define MEMPOOL_OBJECTS 16 +#define ALLOCATION_TIMEOUT_MS 60000 +#define DEFAULT_NODE_ID 60 static void handle_begin_firmware_update(CanardInstance *ins, CanardRxTransfer *transfer); static uint8_t is_node_restart_requested(void); @@ -15,12 +17,12 @@ static CanardInstance g_canard; //The library instance static uint8_t g_canard_memory_pool[2048]; static uint32_t spin_time = 0; uint8_t feedback_status = 0; +static uint8_t node_unique_id[UNIQUE_ID_LENGTH_BYTES]; + //static const uint8_t gimbal_id = 0xC5; //Arena for memory allocation, used by the library //static uint32_t g_uptime = 0; -uint8_t sver_p1 = 1; -uint8_t sver_p2 = 0; -uint8_t sver_p3 = 4; + //static uint8_t node_health = UAVCAN_NODE_HEALTH_OK; //static uint8_t node_mode = UAVCAN_NODE_MODE_INITIALIZATION; static uint64_t send_next_node_id_allocation_request_at; ///< When the next node ID allocation request should be sent @@ -30,6 +32,8 @@ static uint8_t transfer_id = 0; uint32_t node_restart_at; //TODO check where used uint32_t last_invocation; +uint32_t allocation_init_tick; + //static uint32_t last_state_time = 0; //extern QueueHandle_t uavcan_rev_queue; @@ -42,6 +46,12 @@ typedef struct float yaw; // float16 Saturate } zrzk_equipment_uav_Euler; +static struct +{ + uint64_t request_timer_begin_us; + uint64_t request_delay_us; + uint32_t unique_id_offset; +} allocation_state; #ifndef CANARD_INTERNAL_SATURATE #define CANARD_INTERNAL_SATURATE(x, max) (((x) > max) ? max : ((-(x) > max) ? (-max) : (x))); @@ -57,6 +67,21 @@ typedef struct #define CANARD_MAYBE_UNUSED(x) x #endif +extern TIM_HandleTypeDef htim2; + +static void allocation_timer_expired(void); +static void handle_allocation_data_broadcast(CanardInstance *ins, CanardRxTransfer *transfer); +static void allocation_start_request_timer(void); +static void allocation_start_followup_timer(void); +static bool allocation_running(void); +static float uavcan_get_radom_float(void); +uint64_t micros(void); + +static void allocation_init(void); +static void allocation_timer_expired(void); + + + bool shouldAcceptTransfer(const CanardInstance *ins, uint64_t *out_data_type_signature, uint16_t data_type_id, @@ -86,6 +111,19 @@ bool shouldAcceptTransfer(const CanardInstance *ins, case UAVCAN_GET_NODE_INFO_DATA_TYPE_ID: *out_data_type_signature = UAVCAN_GET_NODE_INFO_DATA_TYPE_SIGNATURE; return true; + + case UAVCAN_RESTART_NODE_DATA_TYPE_ID: + *out_data_type_signature = UAVCAN_RESTART_NODE_DATA_TYPE_SIGNATURE; + return true; + case UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID: + *out_data_type_signature = UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_SIGNATURE; + return true; + case UAVCAN_FILE_READ_DATA_TYPE_ID: + *out_data_type_signature = UAVCAN_FILE_READ_DATA_TYPE_SIGNATURE; + return true; + case UAVCAN_PARAM_GETSET_DATA_TYPE_ID: + *out_data_type_signature = UAVCAN_PARAM_GETSET_DATA_TYPE_SIGNATURE; + return true; default: return false; } @@ -116,16 +154,17 @@ void action_transfer_received_request(CanardInstance *ins, CanardRxTransfer *tra handle_canard_get_node_info(transfer); break; - case UAVCAN_RESTART_NODE_DATA_TYPE_ID: - + case UAVCAN_RESTART_NODE_DATA_TYPE_ID: handle_restart_node(transfer); break; + case UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID: handle_begin_firmware_update(ins, transfer); break; + case UAVCAN_PARAM_GETSET_DATA_TYPE_ID: // handle_param_getset(transfer); - break; + break; default: break; } @@ -138,7 +177,7 @@ void action_transfer_received_broadcast(CanardInstance *ins, CanardRxTransfer *t case UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID: if (canardGetLocalNodeID(ins) == CANARD_BROADCAST_NODE_ID) { - handle_allocation_data(&g_canard, transfer); + handle_allocation_data_broadcast(&g_canard, transfer); } break; default: @@ -193,8 +232,7 @@ static void handle_canard_get_node_info(CanardRxTransfer *transfer) } void uavcan_init(void) -{ - +{ CanardSTM32CANTimings timings; int result = canardSTM32ComputeCANTimings(HAL_RCC_GetPCLK1Freq(), 1000000, &timings); if (result) @@ -220,8 +258,7 @@ void uavcan_init(void) } void uavcan_init_with_cubemx(void) -{ - +{ uavcan_init_fifo(uavcan_local_node); canardInit(&g_canard, // Uninitialized library instance g_canard_memory_pool, // Raw memory chunk used for dynamic allocation @@ -234,98 +271,6 @@ void uavcan_init_with_cubemx(void) can_init_irq(); } -void init_allocation(void) -{ - can_filter_none(); - canardInit(&g_canard, // Uninitialized library instance - g_canard_memory_pool, // Raw memory chunk used for dynamic allocation - sizeof(g_canard_memory_pool), // Size of the above, in bytes - onTransferReceived, // Callback, see CanardOnTransferReception - shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer - NULL); - - can_init_irq(); - - //执行动态节点ID分配过程 - - static const uint8_t preferred_node_id = CANARD_BROADCAST_NODE_ID; - node_id_allocation_unique_id_offset = 0; - uint8_t node_id_allocation_transfer_id = 0; - - while (canardGetLocalNodeID(&g_canard) == CANARD_BROADCAST_NODE_ID) - { - //puts("Waiting for dynamic node ID allocation..."); - // vTaskDelay(50); - send_next_node_id_allocation_request_at = - HAL_GetTick() + UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC + - (uint64_t)(get_random_float() * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC); - - // (uint64_t)(0.1 * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC); - - while ((HAL_GetTick() < send_next_node_id_allocation_request_at) && - (canardGetLocalNodeID(&g_canard) == CANARD_BROADCAST_NODE_ID)) - { - send_canard(); - receive_canard(); - osDelay(5); - } - - if (canardGetLocalNodeID(&g_canard) != CANARD_BROADCAST_NODE_ID) - { - break; - } - - // Structure of the request is documented in the DSDL definition 请求的结构记录在DSDL定义中 - // See http://uavcan.org/Specification/6._Application_level_functions/#dynamic-node-id-allocation - - uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; - allocation_request[0] = (uint8_t)(preferred_node_id << 1U); - - if (node_id_allocation_unique_id_offset == 0) - { - allocation_request[0] |= 1; // First part of unique ID - } - - uint8_t my_unique_id[UNIQUE_ID_LENGTH_BYTES] = {0}; - read_unique_id(my_unique_id); - - static const uint8_t max_len_of_unique_id_in_request = 6; - uint8_t uid_size = (uint8_t)(UNIQUE_ID_LENGTH_BYTES - node_id_allocation_unique_id_offset); - if (uid_size > max_len_of_unique_id_in_request) - { - uid_size = max_len_of_unique_id_in_request; - } - - // Paranoia time - assert(node_id_allocation_unique_id_offset < UNIQUE_ID_LENGTH_BYTES); - assert(uid_size <= max_len_of_unique_id_in_request); - assert(uid_size > 0); - assert((uid_size + node_id_allocation_unique_id_offset) <= UNIQUE_ID_LENGTH_BYTES); - - memmove(&allocation_request[1], &my_unique_id[node_id_allocation_unique_id_offset], uid_size); - - // Broadcasting the request - const int16_t bcast_res = canardBroadcast(&g_canard, - UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE, - UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID, - &node_id_allocation_transfer_id, - CANARD_TRANSFER_PRIORITY_LOW, - &allocation_request[0], - (uint16_t)(uid_size + 1)); - if (bcast_res < 0) - { - //(void)fprintf(stderr, "Could not broadcast dynamic node ID allocation request; error %d\n", bcast_res); //TODO ////my_printf() - } - - // Preparing for timeout; if response is received, this value will be updated from the callback. - node_id_allocation_unique_id_offset = 0; - } - uavcan_init_fifo(get_node_id()); - //过滤设置 - can_init_irq(); - -} - void send_canard(void) { osStatus_t status; @@ -408,61 +353,6 @@ uint16_t make_node_info_message(uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX return 41 + name_len; } -//动态节点消息接收处理 //on_allocation_message -void handle_allocation_data(CanardInstance *ins, CanardRxTransfer *transfer) -{ - // Rule C - updating the randomized time interval - send_next_node_id_allocation_request_at = - HAL_GetTick() + UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC + //getMonotonicTimestampUSec() - (uint64_t)(get_random_float() * UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC); - - if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) - { - // puts("Allocation request from another allocatee"); - node_id_allocation_unique_id_offset = 0; - return; - } - - //Copying the unique ID from the message unique_id_bit_offset：位偏移量 --uint8 - static const uint8_t unique_id_bit_offset = 8; - uint8_t received_unique_id[UNIQUE_ID_LENGTH_BYTES]; - uint8_t received_unique_id_len = 0; - - for (; received_unique_id_len < (transfer->payload_len - (unique_id_bit_offset / 8U)); received_unique_id_len++) - { - assert(received_unique_id_len < UNIQUE_ID_LENGTH_BYTES); - const uint8_t bit_offset = (uint8_t)(unique_id_bit_offset + received_unique_id_len * 8U); - (void)canardDecodeScalar(transfer, bit_offset, 8, false, &received_unique_id[received_unique_id_len]); - } - - // Obtaining the local unique ID 获取本地 unique ID - uint8_t my_unique_id[UNIQUE_ID_LENGTH_BYTES] = {0}; - read_unique_id(my_unique_id); - - // Matching the received UID against the local one - if (memcmp(received_unique_id, my_unique_id, received_unique_id_len) != 0) - { - node_id_allocation_unique_id_offset = 0; - - return; // No match, return - } - - if (received_unique_id_len < UNIQUE_ID_LENGTH_BYTES) - { - // The allocator has confirmed part of unique ID, switching to the next stage and updating the timeout. - node_id_allocation_unique_id_offset = received_unique_id_len; - send_next_node_id_allocation_request_at -= UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC; - } - else - { - // Allocation complete - copying the allocated node ID from the message - uint8_t allocated_node_id = 0; - (void)canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id); - assert(allocated_node_id <= 127); - canardSetLocalNodeID(ins, allocated_node_id); - } -} - void canard_get_node_info_handle(CanardRxTransfer *transfer) { uint8_t buffer[UAVCAN_GET_NODE_INFO_RESPONSE_MAX_SIZE]; @@ -618,20 +508,21 @@ static void handle_begin_firmware_update(CanardInstance *ins, CanardRxTransfer * request_node_restart(); } //canardLock(canardReleaseRxTransferPayload(ins, transfer)); - const int resp_res = canardRequestOrRespond(ins, - transfer->source_node_id, - UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_SIGNATURE, - UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &response, - 1); + const int resp_res = + canardRequestOrRespond(ins, + transfer->source_node_id, + UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_SIGNATURE, + UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &response, + 1); } static void read_unique_id(uint8_t *out_uid) { - memcpy(out_uid, (void *)UID_BASE, UNIQUE_ID_LENGTH_BYTES); + memcpy(out_uid, (void *)UID_BASE, UNIQUE_STM32_DEVICE_ID_LENGTH); } uint8_t get_node_id(void) @@ -677,13 +568,221 @@ void handle_restart_node(CanardRxTransfer *transfer) */ request_node_restart(); } - const int resp_res = canardRequestOrRespond( &g_canard, - transfer->source_node_id, - UAVCAN_RESTART_NODE_DATA_TYPE_SIGNATURE, - UAVCAN_RESTART_NODE_DATA_TYPE_ID, - &transfer->transfer_id, - transfer->priority, - CanardResponse, - &response, - 1); + const int resp_res = canardRequestOrRespond(&g_canard, + transfer->source_node_id, + UAVCAN_RESTART_NODE_DATA_TYPE_SIGNATURE, + UAVCAN_RESTART_NODE_DATA_TYPE_ID, + &transfer->transfer_id, + transfer->priority, + CanardResponse, + &response, + 1); +} + +static void allocation_init(void) +{ + if (!allocation_running()) + { + return; + } + + // Start request timer + allocation_start_request_timer(); +} + +void allocation_update(void) +{ + if (!allocation_running()) + { + return; + } + + // Check allocation timer + if (micros() - allocation_state.request_timer_begin_us > allocation_state.request_delay_us) + { + //printf("us:%llu ,ms:%u\r\n", micros(), HAL_GetTick()); + // printf("del:%llu,requst_us:%llu,begin_us:%llu\r\n", + // micros() - allocation_state.request_timer_begin_us, + // allocation_state.request_delay_us, + // allocation_state.request_timer_begin_us); + allocation_timer_expired(); + + // boot_application_at = HAL_GetTick() + 2000; + } + + if ((allocation_init_tick + ALLOCATION_TIMEOUT_MS) < HAL_GetTick()) + { + if (bootloader_interface.magic == BOOTLOADER_INTERFACE_VALID_MAGIC && + bootloader_interface.node_id) + { + canardSetLocalNodeID(&g_canard, bootloader_interface.node_id); + } + else + { + canardSetLocalNodeID(&g_canard, DEFAULT_NODE_ID); + } + } +} + +static void allocation_timer_expired(void) +{ + + if (!allocation_running()) + { + return; + } + + // Start allocation request timer + allocation_start_request_timer(); + + // Send allocation message + uint8_t allocation_request[CANARD_CAN_FRAME_MAX_DATA_LEN - 1]; + uint8_t uid_size = MIN(UNIQUE_ID_LENGTH_BYTES - allocation_state.unique_id_offset, UAVCAN_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UID_IN_REQUEST); + allocation_request[0] = (allocation_state.unique_id_offset == 0) ? 1 : 0; + //printf("allocation_request:%d \r\n",allocation_request[0]); + memcpy(&allocation_request[1], &node_unique_id[allocation_state.unique_id_offset], uid_size); + + static uint8_t transfer_id; + canardBroadcast(&g_canard, UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE, UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID, &transfer_id, CANARD_TRANSFER_PRIORITY_LOW, allocation_request, uid_size + 1); + + allocation_state.unique_id_offset = 0; +} + +static void handle_allocation_data_broadcast(CanardInstance *ins, CanardRxTransfer *transfer) +{ + if (!allocation_running()) + { + return; + } + + // Always start the allocation request timer and reset the UID offset + allocation_start_request_timer(); + allocation_state.unique_id_offset = 0; + + if (transfer->source_node_id == CANARD_BROADCAST_NODE_ID) + { + // If source node ID is anonymous, return + return; + } + + uint8_t received_unique_id[UNIQUE_ID_LENGTH_BYTES]; + uint8_t received_unique_id_len = transfer->payload_len - 1; + uint8_t i; + for (i = 0; i < received_unique_id_len; i++) + { + canardDecodeScalar(transfer, i * 8 + UAVCAN_NODE_ID_ALLOCATION_UID_BIT_OFFSET, 8, false, &received_unique_id[i]); + } + + if (memcmp(node_unique_id, received_unique_id, received_unique_id_len) != 0) + { + // If unique ID does not match, return + return; + } + + if (received_unique_id_len < UNIQUE_ID_LENGTH_BYTES) + { + // Unique ID partially matches - set the UID offset and start the followup timer + allocation_state.unique_id_offset = received_unique_id_len; + allocation_start_followup_timer(); + } + else + { + // Complete match received + uint8_t allocated_node_id = 0; + canardDecodeScalar(transfer, 0, 7, false, &allocated_node_id); + if (allocated_node_id != 0) + { + canardSetLocalNodeID(ins, allocated_node_id); + uavcan_init_fifo(canardGetLocalNodeID(&g_canard)); + } + } +} + +static void allocation_start_request_timer(void) +{ + if (!allocation_running()) + { + return; + } + + allocation_state.request_timer_begin_us = micros(); + allocation_state.request_delay_us = UAVCAN_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_US + (uint32_t)(uavcan_get_radom_float() * (UAVCAN_NODE_ID_ALLOCATION_MAX_REQUEST_PERIOD_US - UAVCAN_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_US)); +} + +static void allocation_start_followup_timer(void) +{ + if (!allocation_running()) + { + return; + } + + allocation_state.request_timer_begin_us = micros(); + allocation_state.request_delay_us = UAVCAN_NODE_ID_ALLOCATION_MIN_FOLLOWUP_PERIOD_US + (uint32_t)(uavcan_get_radom_float() * (UAVCAN_NODE_ID_ALLOCATION_MAX_FOLLOWUP_PERIOD_US - UAVCAN_NODE_ID_ALLOCATION_MIN_FOLLOWUP_PERIOD_US)); +} + +static bool allocation_running(void) +{ + return canardGetLocalNodeID(&g_canard) == CANARD_BROADCAST_NODE_ID; +} + +static float uavcan_get_radom_float(void) +{ + static bool initialized = false; + if (!initialized) + { + initialized = true; + read_unique_id(&node_unique_id[0]); + + const uint32_t *unique_32 = (uint32_t *)&node_unique_id[0]; + + srand(micros() ^ *unique_32); + } + + return (float)rand() / (float)RAND_MAX; +} + +void uavcan_init_by_allocation(void) +{ + can_filter_none(); + can_init_irq(); + canardInit(&g_canard, // Uninitialized library instance + g_canard_memory_pool, // Raw memory chunk used for dynamic allocation + sizeof(g_canard_memory_pool), // Size of the above, in bytes + onTransferReceived, // Callback, see CanardOnTransferReception + shouldAcceptTransfer, // Callback, see CanardShouldAcceptTransfer + NULL); + can_init_irq(); + read_unique_id(&node_unique_id[0]); + allocation_init(); + // canard_initialized = true; + allocation_init_tick = HAL_GetTick(); +} + +uint64_t micros(void) +{ + return HAL_GetTick()*1000 + __HAL_TIM_GetCounter(&htim2); } + +void init_uavcan_fifo(uint8_t node_id) +{ + uint32_t id = 0; + uint32_t mask = 0; + uint8_t bank_index = 1; + + uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID, &id, &mask); + can_filter_init_with_mask(bank_index, id, mask, 1); + bank_index++; + + uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_FILE_READ_DATA_TYPE_ID, &id, &mask); + can_filter_init_with_mask(bank_index, id, mask, 0); + bank_index++; + + uavcan_get_uavcan_id_mask_by_msg_id(UAVCAN_RESTART_NODE_DATA_TYPE_ID, &id, &mask); + can_filter_init_with_mask(bank_index, id, mask, 1); + bank_index++; + + uavcan_get_uavcan_id_mask_by_node_id(node_id, &id, &mask); + can_filter_init_with_mask(bank_index, id, mask, 1); + bank_index++; +} + diff --git a/UAVCAN/uavcan.h b/UAVCAN/uavcan.h index 5a1a9ac..add76fe 100644 --- a/UAVCAN/uavcan.h +++ b/UAVCAN/uavcan.h @@ -20,7 +20,7 @@ extern "C" #define ARRAY_SIZE(arr) (sizeof(arr) / sizeof(arr[0])) #define APP_VERSION_MAJOR 1 -#define APP_VERSION_MINOR 0 +#define APP_VERSION_MINOR 1 #define HARDWARE_VERSION_MAJOR 1 #define APP_NODE_NAME "zrzk.proxi.1" #define GIT_HASH 0xBADC0FFE @@ -49,45 +49,53 @@ extern "C" #define UAVCAN_DEBUG_LOG_MESSAGE_DATA_TYPE_SIGNATURE 0xd654a48e0c049d75 #define UAVCAN_DEBUG_LOG_MESSAGE_MESSAGE_SIZE ((983 + 7) / 8) -#define UNIQUE_ID_LENGTH_BYTES 12 +#define UNIQUE_ID_LENGTH_BYTES 16 +#define UNIQUE_STM32_DEVICE_ID_LENGTH 12 -#define UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID 1 -#define UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE 0x0b2a812620a11d40 -#define UAVCAN_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_US 600U -#define UAVCAN_NODE_ID_ALLOCATION_MAX_REQUEST_PERIOD_US 1000U -#define UAVCAN_NODE_ID_ALLOCATION_MIN_FOLLOWUP_PERIOD_US 0U -#define UAVCAN_NODE_ID_ALLOCATION_MAX_FOLLOWUP_PERIOD_US 400U -#define UAVCAN_NODE_ID_ALLOCATION_UID_BIT_OFFSET 8 -#define UAVCAN_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UID_IN_REQUEST 6 + // #define UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID 1 + // #define UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE 0x0b2a812620a11d40 + // #define UAVCAN_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_US 600U + // #define UAVCAN_NODE_ID_ALLOCATION_MAX_REQUEST_PERIOD_US 1000U + // #define UAVCAN_NODE_ID_ALLOCATION_MIN_FOLLOWUP_PERIOD_US 0U + // #define UAVCAN_NODE_ID_ALLOCATION_MAX_FOLLOWUP_PERIOD_US 400U + // #define UAVCAN_NODE_ID_ALLOCATION_UID_BIT_OFFSET 8 + // #define UAVCAN_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UID_IN_REQUEST 6 -#define UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC 400UL -#define UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC 600UL + // #define UAVCAN_NODE_ID_ALLOCATION_RANDOM_TIMEOUT_RANGE_USEC 400UL + // #define UAVCAN_NODE_ID_ALLOCATION_REQUEST_DELAY_OFFSET_USEC 600UL #define UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_ID 16383 #define UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_SIGNATURE (0xD654A48E0C049D75ULL) #define UAVCAN_PROTOCOL_DEBUG_LOGMESSAGE_MAX_SIZE ((983 + 7) / 8) +#define UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID 40 +#define UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_SIGNATURE 0xb7d725df72724126 +#define UAVCAN_BEGIN_FIRMWARE_UPDATE_RESPONSE_MAX_SIZE ((1031 + 7) / 8) +#define UAVCAN_BEGIN_FIRMWARE_UPDATE_REQUEST_MAX_SIZE ((1616 + 7) / 8) -#define UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_ID 40 -#define UAVCAN_BEGIN_FIRMWARE_UPDATE_DATA_TYPE_SIGNATURE 0xb7d725df72724126 -#define UAVCAN_BEGIN_FIRMWARE_UPDATE_RESPONSE_MAX_SIZE ((1031+7)/8) -#define UAVCAN_BEGIN_FIRMWARE_UPDATE_REQUEST_MAX_SIZE ((1616+7)/8) - -#define UAVCAN_FILE_READ_DATA_TYPE_ID 48 -#define UAVCAN_FILE_READ_DATA_TYPE_SIGNATURE 0x8dcdca939f33f678 -#define UAVCAN_FILE_READ_REQUEST_MAX_SIZE ((1648+7)/8) -#define UAVCAN_FILE_READ_RESPONSE_MAX_SIZE ((2073+7)/8) +#define UAVCAN_FILE_READ_DATA_TYPE_ID 48 +#define UAVCAN_FILE_READ_DATA_TYPE_SIGNATURE 0x8dcdca939f33f678 +#define UAVCAN_FILE_READ_REQUEST_MAX_SIZE ((1648 + 7) / 8) +#define UAVCAN_FILE_READ_RESPONSE_MAX_SIZE ((2073 + 7) / 8) -#define UAVCAN_RESTART_NODE_DATA_TYPE_ID 5 -#define UAVCAN_RESTART_NODE_DATA_TYPE_SIGNATURE 0x569e05394a3017f0 -#define UAVCAN_RESTART_NODE_REQUEST_MAX_SIZE ((40+7)/8) -#define UAVCAN_RESTART_NODE_RESPONSE_MAX_SIZE ((1+7)/8) +#define UAVCAN_RESTART_NODE_DATA_TYPE_ID 5 +#define UAVCAN_RESTART_NODE_DATA_TYPE_SIGNATURE 0x569e05394a3017f0 +#define UAVCAN_RESTART_NODE_REQUEST_MAX_SIZE ((40 + 7) / 8) +#define UAVCAN_RESTART_NODE_RESPONSE_MAX_SIZE ((1 + 7) / 8) -#define UAVCAN_PARAM_GETSET_REQUEST_MAX_SIZE ((1791+7)/8) -#define UAVCAN_PARAM_GETSET_RESPONSE_MAX_SIZE ((2967+7)/8) -#define UAVCAN_PARAM_GETSET_DATA_TYPE_SIGNATURE 0xa7b622f939d1a4d5 -#define UAVCAN_PARAM_GETSET_DATA_TYPE_ID 11 +#define UAVCAN_PARAM_GETSET_REQUEST_MAX_SIZE ((1791 + 7) / 8) +#define UAVCAN_PARAM_GETSET_RESPONSE_MAX_SIZE ((2967 + 7) / 8) +#define UAVCAN_PARAM_GETSET_DATA_TYPE_SIGNATURE 0xa7b622f939d1a4d5 +#define UAVCAN_PARAM_GETSET_DATA_TYPE_ID 11 +#define UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_ID 1 +#define UAVCAN_NODE_ID_ALLOCATION_DATA_TYPE_SIGNATURE 0x0b2a812620a11d40 +#define UAVCAN_NODE_ID_ALLOCATION_MIN_REQUEST_PERIOD_US 600000U +#define UAVCAN_NODE_ID_ALLOCATION_MAX_REQUEST_PERIOD_US 1000000U +#define UAVCAN_NODE_ID_ALLOCATION_MIN_FOLLOWUP_PERIOD_US 0U +#define UAVCAN_NODE_ID_ALLOCATION_MAX_FOLLOWUP_PERIOD_US 400000U +#define UAVCAN_NODE_ID_ALLOCATION_UID_BIT_OFFSET 8 +#define UAVCAN_NODE_ID_ALLOCATION_MAX_LENGTH_OF_UID_IN_REQUEST 6 #define RCPWM_PERIOD_uS 20000 #define RCPWM_NEUTRAL_uS 1500 @@ -110,6 +118,11 @@ extern "C" #define USE_CAN_LOG 1 +#undef MIN +#undef MAX +#define MIN(a, b) (((a) < (b)) ? (a) : (b)) +#define MAX(a, b) (((a) > (b)) ? (a) : (b)) + static const uint8_t uavcan_local_node = 65; typedef struct @@ -226,7 +239,8 @@ extern "C" void uavcan_init_fifo(uint8_t node_id); void uavcan_cleanup_stale_transfers(uint64_t time_us); void handle_restart_node(CanardRxTransfer *transfer); - + void uavcan_init_by_allocation(void); + void allocation_update(void); #ifdef __cplusplus } #endif diff --git a/UAVCAN/uavcan_filter.c b/UAVCAN/uavcan_filter.c index 31c7a46..bb69770 100644 --- a/UAVCAN/uavcan_filter.c +++ b/UAVCAN/uavcan_filter.c @@ -81,3 +81,5 @@ void uavcan_get_uavcan_id_mask_by_node_id(uint32_t node_id, uint32_t *id, uint32 } + + diff --git a/proximity_uavcan_converter.ioc b/proximity_uavcan_converter.ioc index 5b836f4..9ad1ebe 100644 --- a/proximity_uavcan_converter.ioc +++ b/proximity_uavcan_converter.ioc @@ -51,7 +51,7 @@ Dma.USART2_TX.1.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphData FREERTOS.FootprintOK=true FREERTOS.IPParameters=Tasks01,Queues01,configTOTAL_HEAP_SIZE,configQUEUE_REGISTRY_SIZE,configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY,FootprintOK,Mutexes01 FREERTOS.Mutexes01=uavcan_send_mutex,Dynamic,NULL;radar_parse_mutex,Dynamic,NULL -FREERTOS.Queues01=uavcan_rev_queue,10,uavcan_rev_t,0,Dynamic,NULL,NULL;uavcan_send_queue,5,uavcan_send_data_t,0,Dynamic,NULL,NULL;usart_rev_queue,10,usart_data_t,0,Dynamic,NULL,NULL;proximity1_queue,5,proximity_data_t,0,Dynamic,NULL,NULL;proximity2_queue,5,proximity_data_t,0,Dynamic,NULL,NULL +FREERTOS.Queues01=uavcan_rev_queue,10,uavcan_rev_t,0,Dynamic,NULL,NULL;uavcan_send_queue,3,uavcan_send_data_t,0,Dynamic,NULL,NULL;usart_rev_queue,4,usart_data_t,0,Dynamic,NULL,NULL;proximity1_queue,2,proximity_data_t,0,Dynamic,NULL,NULL;proximity2_queue,2,proximity_data_t,0,Dynamic,NULL,NULL;usart_send_queue,8,usart_data_t,0,Dynamic,NULL,NULL FREERTOS.Tasks01=DefaultTask,24,256,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL;UavcanRevTask,35,256,StartUavcanRevTask,Default,NULL,Dynamic,NULL,NULL;UsartSendTask,18,128,StartTaskUsartSend,Default,NULL,Dynamic,NULL,NULL;UavcanSendTask,36,512,StartUavcanSendTask,Default,NULL,Dynamic,NULL,NULL;UsartRevTask,39,256,StartUsartRevTask,Default,NULL,Dynamic,NULL,NULL;proximityTask,36,256,StartProximity,Default,NULL,Dynamic,NULL,NULL FREERTOS.configLIBRARY_MAX_SYSCALL_INTERRUPT_PRIORITY=5 FREERTOS.configQUEUE_REGISTRY_SIZE=40