From 00cfee37c721f8e1cffcb180a8bf5fbd5b39811b Mon Sep 17 00:00:00 2001 From: Robofish <1683502971@qq.com> Date: Fri, 2 Jan 2026 23:29:19 +0800 Subject: [PATCH] =?UTF-8?q?=E7=94=9F=E6=88=90bsp?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .DS_Store | Bin 10244 -> 6148 bytes CMakeLists.txt | 27 ++ Core/Src/freertos.c | 256 ++++++------ User/bsp/bsp.h | 28 ++ User/bsp/bsp_config.yaml | 110 +++++ User/bsp/fdcan.c | 576 +++++++++++++++++++++++++++ User/bsp/fdcan.h | 137 +++++++ User/bsp/flash.c | 82 ++++ User/bsp/flash.h | 46 +++ User/bsp/gpio.c | 123 ++++++ User/bsp/gpio.h | 56 +++ User/bsp/mm.c | 30 ++ User/bsp/mm.h | 32 ++ User/bsp/pwm.c | 115 ++++++ User/bsp/pwm.h | 53 +++ User/bsp/spi.c | 181 +++++++++ User/bsp/spi.h | 70 ++++ User/bsp/time.c | 81 ++++ User/bsp/time.h | 43 ++ User/bsp/uart.c | 159 ++++++++ User/bsp/uart.h | 69 ++++ User/component/ahrs.c | 417 +++++++++++++++++++ User/component/ahrs.h | 114 ++++++ User/component/component_config.yaml | 7 + User/component/user_math.c | 134 +++++++ User/component/user_math.h | 179 +++++++++ User/device/bmi088.c | 381 ++++++++++++++++++ User/device/bmi088.h | 81 ++++ User/device/device.h | 51 +++ User/device/device_config.yaml | 15 + User/device/dr16.c | 169 ++++++++ User/device/dr16.h | 117 ++++++ User/device/motor.c | 52 +++ User/device/motor.h | 68 ++++ User/module/config.c | 38 ++ User/module/config.h | 38 ++ User/task/ai.c | 44 ++ User/task/config.yaml | 7 + User/task/init.c | 42 ++ User/task/user_task.c | 16 + User/task/user_task.h | 80 ++++ 41 files changed, 4195 insertions(+), 129 deletions(-) create mode 100644 User/bsp/bsp.h create mode 100644 User/bsp/bsp_config.yaml create mode 100644 User/bsp/fdcan.c create mode 100644 User/bsp/fdcan.h create mode 100644 User/bsp/flash.c create mode 100644 User/bsp/flash.h create mode 100644 User/bsp/gpio.c create mode 100644 User/bsp/gpio.h create mode 100644 User/bsp/mm.c create mode 100644 User/bsp/mm.h create mode 100644 User/bsp/pwm.c create mode 100644 User/bsp/pwm.h create mode 100644 User/bsp/spi.c create mode 100644 User/bsp/spi.h create mode 100644 User/bsp/time.c create mode 100644 User/bsp/time.h create mode 100644 User/bsp/uart.c create mode 100644 User/bsp/uart.h create mode 100644 User/component/ahrs.c create mode 100644 User/component/ahrs.h create mode 100644 User/component/component_config.yaml create mode 100644 User/component/user_math.c create mode 100644 User/component/user_math.h create mode 100644 User/device/bmi088.c create mode 100644 User/device/bmi088.h create mode 100644 User/device/device.h create mode 100644 User/device/device_config.yaml create mode 100644 User/device/dr16.c create mode 100644 User/device/dr16.h create mode 100644 User/device/motor.c create mode 100644 User/device/motor.h create mode 100644 User/module/config.c create mode 100644 User/module/config.h create mode 100644 User/task/ai.c create mode 100644 User/task/config.yaml create mode 100644 User/task/init.c create mode 100644 User/task/user_task.c create mode 100644 User/task/user_task.h diff --git a/.DS_Store b/.DS_Store index e920e94f60558a5617aed04d08a696909d952c08..1e6711682c1c6c78942d9f90bd5929b691ed523b 100644 GIT binary patch delta 205 zcmZn(XfcprU|?W$DortDU=RQ@Ie-{Mvv5r;6q~50$jGxXU^g=(&tx6}Yu5ch37yG4 z0;;SbK-P@OYk;h!K$hO*9zk=)qmz#aIx?D0-Xo;WssR+8H(5eh6DTSsYQn}i1t_p+ z@;*^r#?Z-cM71X;ipc?$Hi$`2ek`WLDhf8lKth=@baRAcDdS>x4h}(Ppdb(ka03Zf ekc&1JerKM{ui^=E9|IG_tsv(!Y>wxd!wdisu_);P literal 10244 zcmeHM>rPWa5T3;nYPkpjVk9Ou{?V9708RW`q2&_6s71p+>_`uk zW*2D7Kjgp_(Kzh%pczp43eCx1ny}rF{elxJxG427V!uRnMD>X(AmlV4{Gqgc_+8?e8z*Vuku?$OR)D2bMwM2j-i_ zpY3lK7>>CU?@ze5v2&KcHyAT`L(}*ZTsNuVdKY$W3n}!k3D)YY3$I5&2Qe6%p(%rD zf#p7sC1JNktq_jK$M&^2#BsLpj?txW&cU+uIyPzjS-7b~ zay~H2YwIWvNv zNOsTDF1@99KX0 za8{@Gp7vx6_`XihkXJ|aoL1-ss@oa(3|A}8`RiJ$!pa(K##)Nh^##hK z-+L0nritMITpWUjUGy$b+lVLfRgtW+BeLymn+OghPkw7B!s>lwRRC6M;H3b`^@;0o z(ai62bI*I-4e-g|gzL=myTW*RXmj`-f<3O#J&X$5yDmd!kPuIK(@~9y5g1;5`e!IRU4{VD|S6XbrUlZ}+Hw_y9CJ1pQY}+WR+tMoEeQp&ql;-{a-01)R-?+GaK%N25z~5s)C9?S} z|9CZf>k-RX8q0S4NArgD25681!HHG)Mz-T|!`tzX&9fDjeBgb?$bpsu$8&*0>fQW* Y{?7ov{`c$u;XXF6i=B1bf3g1m32mM#9RL6T diff --git a/CMakeLists.txt b/CMakeLists.txt index 3721242..e5cadc8 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -45,11 +45,38 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE # Add sources to executable target_sources(${CMAKE_PROJECT_NAME} PRIVATE # Add user sources here + # User/bsp sources + User/bsp/fdcan.c + User/bsp/flash.c + User/bsp/gpio.c + User/bsp/mm.c + User/bsp/pwm.c + User/bsp/spi.c + User/bsp/time.c + User/bsp/uart.c + + # User/component sources + User/component/ahrs.c + User/component/user_math.c + + # User/device sources + User/device/bmi088.c + User/device/dr16.c + User/device/motor.c + + # User/module sources + User/module/config.c + + # User/task sources + User/task/ai.c + User/task/init.c + User/task/user_task.c ) # Add include paths target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE # Add user defined include paths + User ) # Add project symbols (macros) diff --git a/Core/Src/freertos.c b/Core/Src/freertos.c index cdb9c31..8d21e2e 100644 --- a/Core/Src/freertos.c +++ b/Core/Src/freertos.c @@ -1,129 +1,127 @@ -/* USER CODE BEGIN Header */ -/** - ****************************************************************************** - * File Name : freertos.c - * Description : Code for freertos applications - ****************************************************************************** - * @attention - * - * Copyright (c) 2026 STMicroelectronics. - * All rights reserved. - * - * This software is licensed under terms that can be found in the LICENSE file - * in the root directory of this software component. - * If no LICENSE file comes with this software, it is provided AS-IS. - * - ****************************************************************************** - */ -/* USER CODE END Header */ - -/* Includes ------------------------------------------------------------------*/ -#include "FreeRTOS.h" -#include "task.h" -#include "main.h" -#include "cmsis_os.h" - -/* Private includes ----------------------------------------------------------*/ -/* USER CODE BEGIN Includes */ - -/* USER CODE END Includes */ - -/* Private typedef -----------------------------------------------------------*/ -/* USER CODE BEGIN PTD */ - -/* USER CODE END PTD */ - -/* Private define ------------------------------------------------------------*/ -/* USER CODE BEGIN PD */ - -/* USER CODE END PD */ - -/* Private macro -------------------------------------------------------------*/ -/* USER CODE BEGIN PM */ - -/* USER CODE END PM */ - -/* Private variables ---------------------------------------------------------*/ -/* USER CODE BEGIN Variables */ - -/* USER CODE END Variables */ -/* Definitions for defaultTask */ -osThreadId_t defaultTaskHandle; -const osThreadAttr_t defaultTask_attributes = { - .name = "defaultTask", - .stack_size = 128 * 4, - .priority = (osPriority_t) osPriorityNormal, -}; - -/* Private function prototypes -----------------------------------------------*/ -/* USER CODE BEGIN FunctionPrototypes */ - -/* USER CODE END FunctionPrototypes */ - -void StartDefaultTask(void *argument); - -void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ - -/** - * @brief FreeRTOS initialization - * @param None - * @retval None - */ -void MX_FREERTOS_Init(void) { - /* USER CODE BEGIN Init */ - - /* USER CODE END Init */ - - /* USER CODE BEGIN RTOS_MUTEX */ - /* add mutexes, ... */ - /* USER CODE END RTOS_MUTEX */ - - /* USER CODE BEGIN RTOS_SEMAPHORES */ - /* add semaphores, ... */ - /* USER CODE END RTOS_SEMAPHORES */ - - /* USER CODE BEGIN RTOS_TIMERS */ - /* start timers, add new ones, ... */ - /* USER CODE END RTOS_TIMERS */ - - /* USER CODE BEGIN RTOS_QUEUES */ - /* add queues, ... */ - /* USER CODE END RTOS_QUEUES */ - - /* Create the thread(s) */ - /* creation of defaultTask */ - defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes); - - /* USER CODE BEGIN RTOS_THREADS */ - /* add threads, ... */ - /* USER CODE END RTOS_THREADS */ - - /* USER CODE BEGIN RTOS_EVENTS */ - /* add events, ... */ - /* USER CODE END RTOS_EVENTS */ - -} - -/* USER CODE BEGIN Header_StartDefaultTask */ -/** - * @brief Function implementing the defaultTask thread. - * @param argument: Not used - * @retval None - */ -/* USER CODE END Header_StartDefaultTask */ -void StartDefaultTask(void *argument) -{ - /* USER CODE BEGIN StartDefaultTask */ - /* Infinite loop */ - for(;;) - { - osDelay(1); - } - /* USER CODE END StartDefaultTask */ -} - -/* Private application code --------------------------------------------------*/ -/* USER CODE BEGIN Application */ - -/* USER CODE END Application */ - +/* USER CODE BEGIN Header */ +/** + ****************************************************************************** + * File Name : freertos.c + * Description : Code for freertos applications + ****************************************************************************** + * @attention + * + * Copyright (c) 2026 STMicroelectronics. + * All rights reserved. + * + * This software is licensed under terms that can be found in the LICENSE file + * in the root directory of this software component. + * If no LICENSE file comes with this software, it is provided AS-IS. + * + ****************************************************************************** + */ +/* USER CODE END Header */ + +/* Includes ------------------------------------------------------------------*/ +#include "FreeRTOS.h" +#include "task.h" +#include "main.h" +#include "cmsis_os.h" + +/* Private includes ----------------------------------------------------------*/ +/* USER CODE BEGIN Includes */ + +#include "task/user_task.h" +/* USER CODE END Includes */ + +/* Private typedef -----------------------------------------------------------*/ +/* USER CODE BEGIN PTD */ + +/* USER CODE END PTD */ + +/* Private define ------------------------------------------------------------*/ +/* USER CODE BEGIN PD */ + +/* USER CODE END PD */ + +/* Private macro -------------------------------------------------------------*/ +/* USER CODE BEGIN PM */ + +/* USER CODE END PM */ + +/* Private variables ---------------------------------------------------------*/ +/* USER CODE BEGIN Variables */ + +/* USER CODE END Variables */ +/* Definitions for defaultTask */ +osThreadId_t defaultTaskHandle; +const osThreadAttr_t defaultTask_attributes = { + .name = "defaultTask", + .stack_size = 128 * 4, + .priority = (osPriority_t) osPriorityNormal, +}; + +/* Private function prototypes -----------------------------------------------*/ +/* USER CODE BEGIN FunctionPrototypes */ + +/* USER CODE END FunctionPrototypes */ + +void StartDefaultTask(void *argument); + +void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ + +/** + * @brief FreeRTOS initialization + * @param None + * @retval None + */ +void MX_FREERTOS_Init(void) { + /* USER CODE BEGIN Init */ + + /* USER CODE END Init */ + + /* USER CODE BEGIN RTOS_MUTEX */ + /* add mutexes, ... */ + /* USER CODE END RTOS_MUTEX */ + + /* USER CODE BEGIN RTOS_SEMAPHORES */ + /* add semaphores, ... */ + /* USER CODE END RTOS_SEMAPHORES */ + + /* USER CODE BEGIN RTOS_TIMERS */ + /* start timers, add new ones, ... */ + /* USER CODE END RTOS_TIMERS */ + + /* USER CODE BEGIN RTOS_QUEUES */ + /* add queues, ... */ + /* USER CODE END RTOS_QUEUES */ + + /* Create the thread(s) */ + /* creation of defaultTask */ + defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes); + + /* USER CODE BEGIN RTOS_THREADS */ + /* add threads, ... */ + osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务 +/* USER CODE END RTOS_THREADS */ + + /* USER CODE BEGIN RTOS_EVENTS */ + /* add events, ... */ + /* USER CODE END RTOS_EVENTS */ + +} + +/* USER CODE BEGIN Header_StartDefaultTask */ +/** + * @brief Function implementing the defaultTask thread. + * @param argument: Not used + * @retval None + */ +/* USER CODE END Header_StartDefaultTask */ +void StartDefaultTask(void *argument) +{ + /* USER CODE BEGIN StartDefaultTask */ + osThreadTerminate(osThreadGetId()); +/* USER CODE END StartDefaultTask */ +} + +/* Private application code --------------------------------------------------*/ +/* USER CODE BEGIN Application */ + +/* USER CODE END Application */ + diff --git a/User/bsp/bsp.h b/User/bsp/bsp.h new file mode 100644 index 0000000..1f8cbfd --- /dev/null +++ b/User/bsp/bsp.h @@ -0,0 +1,28 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +#define BSP_OK (0) +#define BSP_ERR (-1) +#define BSP_ERR_NULL (-2) +#define BSP_ERR_INITED (-3) +#define BSP_ERR_NO_DEV (-4) +#define BSP_ERR_TIMEOUT (-5) + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/bsp_config.yaml b/User/bsp/bsp_config.yaml new file mode 100644 index 0000000..02ceebf --- /dev/null +++ b/User/bsp/bsp_config.yaml @@ -0,0 +1,110 @@ +fdcan: + devices: + - instance: FDCAN1 + name: '1' + - instance: FDCAN2 + name: '2' + - instance: FDCAN3 + name: '3' + enabled: true +flash: + dual_bank: false + enabled: true + mcu_name: STM32H723VGTx + sectors: 8 +gpio: + configs: + - custom_name: LCD_BLK + has_exti: false + ioc_label: LCD_BLK + pin: PB10 + type: OUTPUT + - custom_name: LCD_RES + has_exti: false + ioc_label: LCD_RES + pin: PB11 + type: OUTPUT + - custom_name: ACC_CS + has_exti: false + ioc_label: ACC_CS + pin: PC0 + type: OUTPUT + - custom_name: POWER_24V_2 + has_exti: false + ioc_label: POWER_24V_2 + pin: PC13 + type: OUTPUT + - custom_name: POWER_5V + has_exti: false + ioc_label: POWER_5V + pin: PC15-OSC32_OUT + type: OUTPUT + - custom_name: GYRO_CS + has_exti: false + ioc_label: GYRO_CS + pin: PC3_C + type: OUTPUT + - custom_name: ACC_INT + has_exti: true + ioc_label: ACC_INT + pin: PE10 + type: EXTI + - custom_name: W25Q64_CS + has_exti: false + ioc_label: W25Q64_CS + pin: PE11 + type: OUTPUT + - custom_name: GYRO_INT + has_exti: true + ioc_label: GYRO_INT + pin: PE12 + type: EXTI + - custom_name: LCD_CS + has_exti: false + ioc_label: LCD_CS + pin: PE15 + type: OUTPUT + enabled: true +mm: + enabled: true +pwm: + configs: + - channel: TIM_CHANNEL_1 + custom_name: TIM2_CH1 + label: TIM2_CH1 + timer: TIM2 + - channel: TIM_CHANNEL_3 + custom_name: TIM2_CH3 + label: TIM2_CH3 + timer: TIM2 + - channel: TIM_CHANNEL_4 + custom_name: TIM3_CH4 + label: TIM3_CH4 + timer: TIM3 + - channel: TIM_CHANNEL_2 + custom_name: TIM12_CH2 + label: TIM12_CH2 + timer: TIM12 + - channel: TIM_CHANNEL_3 + custom_name: TIM1_CH3 + label: TIM1_CH3 + timer: TIM1 + - channel: TIM_CHANNEL_1 + custom_name: TIM1_CH1 + label: TIM1_CH1 + timer: TIM1 + enabled: true +spi: + devices: + - instance: SPI2 + name: BMI088 + enabled: true +time: + enabled: true +uart: + devices: + - instance: UART5 + name: DR16 + - instance: USART2 + name: IMU + enabled: true diff --git a/User/bsp/fdcan.c b/User/bsp/fdcan.c new file mode 100644 index 0000000..6c6221f --- /dev/null +++ b/User/bsp/fdcan.c @@ -0,0 +1,576 @@ +/* Includes ----------------------------------------------------------------- */ +#include "fdcan.h" +#include "bsp/fdcan.h" +#include "bsp/bsp.h" +#include +#include +#include + +/* Private define ----------------------------------------------------------- */ +#define FDCAN_QUEUE_MUTEX_TIMEOUT 100 + +/* Private macro ------------------------------------------------------------ */ + +/* ===== FDCAN_FilterTypeDef 配置表 ===== + * 定义每个FDCAN实例的过滤器参数表。 + * 过滤器表参数说明： + * idx idtype ftype id1 id2 rxidx + * 过滤器编号 标识符类型 过滤器类型 过滤器ID1 过滤器ID2 接收缓冲区索引 + */ +#ifdef FDCAN1_EN + #define FDCAN1_FILTER_CONFIG_TABLE(X) \ + X(0, FDCAN_STANDARD_ID, FDCAN_FILTER_MASK, 0x000 , 0x000 , 0) \ + X(1, FDCAN_EXTENDED_ID, FDCAN_FILTER_MASK, 0x00000000, 0x00000000, 0) + #define FDCAN1_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */ +#endif +#ifdef FDCAN2_EN + #define FDCAN2_FILTER_CONFIG_TABLE(X) \ + X(0, FDCAN_STANDARD_ID, FDCAN_FILTER_MASK, 0x000 , 0x000 , 0) \ + X(1, FDCAN_EXTENDED_ID, FDCAN_FILTER_MASK, 0x00000000, 0x00000000, 0) + #define FDCAN2_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */ +#endif +#ifdef FDCAN3_EN + #define FDCAN3_FILTER_CONFIG_TABLE(X) \ + X(0, FDCAN_STANDARD_ID, FDCAN_FILTER_MASK, 0x000 , 0x000 , 0) \ + X(1, FDCAN_EXTENDED_ID, FDCAN_FILTER_MASK, 0x00000000, 0x00000000, 0) + #define FDCAN3_GLOBAL_FILTER FDCAN_REJECT, FDCAN_REJECT, FDCAN_FILTER_REMOTE, FDCAN_FILTER_REMOTE/* 全局过滤器参数（用于 HAL_FDCAN_ConfigGlobalFilter） */ +#endif + +/* ====宏展开实现==== */ +#define FDCAN_FILTER_TO_RXFIFO_ENUM_INNER(FIFOIndex) FDCAN_FILTER_TO_RXFIFO##FIFOIndex +#define FDCAN_FILTER_TO_RXFIFO_ENUM(FIFOIndex) FDCAN_FILTER_TO_RXFIFO_ENUM_INNER(FIFOIndex) +#define FDCAN_CONFIG_FILTER(idx, idtype, ftype, id1, id2, rxidx) \ + sFilterConfig.FilterIndex = (idx); \ + sFilterConfig.IdType = (idtype); \ + sFilterConfig.FilterType = (ftype); \ + sFilterConfig.FilterConfig = (FDCAN_FILTER_TO_RXFIFO_ENUM(FDCANX_RX_FIFO)); \ + sFilterConfig.FilterID1 = (id1); \ + sFilterConfig.FilterID2 = (id2); \ + sFilterConfig.RxBufferIndex = (rxidx); \ + HAL_FDCAN_ConfigFilter(&hfdcan, &sFilterConfig); + +#define FDCAN_NOTIFY_FLAG_RXFIFO_INNER(FIFO_IDX) FDCAN_IT_RX_FIFO##FIFO_IDX##_NEW_MESSAGE +#define FDCAN_NOTIFY_FLAG_RXFIFO(FIFO_IDX) FDCAN_NOTIFY_FLAG_RXFIFO_INNER(FIFO_IDX) +#define FDCANx_NOTIFY_FLAGS(FIFO_MACRO) (FDCAN_NOTIFY_FLAG_RXFIFO(FIFO_MACRO) | FDCAN_IT_TX_EVT_FIFO_NEW_DATA | FDCAN_IT_RAM_ACCESS_FAILURE) + +#define FDCANX_MSG_PENDING_CB_INNER(FIFO_IDX) HAL_FDCAN_RX_FIFO##FIFO_IDX##_MSG_PENDING_CB +#define FDCANX_MSG_PENDING_CB(FIFO_IDX) FDCANX_MSG_PENDING_CB_INNER(FIFO_IDX) +/* Private typedef ---------------------------------------------------------- */ +typedef struct BSP_FDCAN_QueueNode { + BSP_FDCAN_t fdcan; + uint32_t can_id; + osMessageQueueId_t queue; + uint8_t queue_size; + struct BSP_FDCAN_QueueNode *next; +} BSP_FDCAN_QueueNode_t; + +/* Private variables -------------------------------------------------------- */ +static BSP_FDCAN_QueueNode_t *queue_list = NULL; +static osMutexId_t queue_mutex = NULL; +static void (*FDCAN_Callback[BSP_FDCAN_NUM][HAL_FDCAN_CB_NUM])(void); +static bool inited = false; +static BSP_FDCAN_IdParser_t id_parser = NULL; +static BSP_FDCAN_TxQueue_t tx_queues[BSP_FDCAN_NUM]; +static const uint8_t fdcan_dlc2len[16] = {0,1,2,3,4,5,6,7,8,12,16,20,24,32,48,64}; + +/* Private function prototypes ---------------------------------------------- */ +static BSP_FDCAN_t FDCAN_Get(FDCAN_HandleTypeDef *hfdcan); +static osMessageQueueId_t BSP_FDCAN_FindQueue(BSP_FDCAN_t fdcan, uint32_t can_id); +static int8_t BSP_FDCAN_CreateIdQueue(BSP_FDCAN_t fdcan, uint32_t can_id, uint8_t queue_size); +static void BSP_FDCAN_RxFifo0Callback(void); +static void BSP_FDCAN_RxFifo1Callback(void); +static void BSP_FDCAN_TxCompleteCallback(void); +static BSP_FDCAN_FrameType_t BSP_FDCAN_GetFrameType(FDCAN_RxHeaderTypeDef *header); +static uint32_t BSP_FDCAN_DefaultIdParser(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type); +static void BSP_FDCAN_TxQueueInit(BSP_FDCAN_t fdcan); +static bool BSP_FDCAN_TxQueuePush(BSP_FDCAN_t fdcan, BSP_FDCAN_TxMessage_t *msg); +static bool BSP_FDCAN_TxQueuePop(BSP_FDCAN_t fdcan, BSP_FDCAN_TxMessage_t *msg); +static bool BSP_FDCAN_TxQueueIsEmpty(BSP_FDCAN_t fdcan); + +/* Private functions -------------------------------------------------------- */ +static BSP_FDCAN_t FDCAN_Get(FDCAN_HandleTypeDef *hfdcan) { + if (hfdcan == NULL) return BSP_FDCAN_ERR; + if (hfdcan->Instance == FDCAN1) return BSP_FDCAN_1; + else if (hfdcan->Instance == FDCAN2) return BSP_FDCAN_2; + else if (hfdcan->Instance == FDCAN3) return BSP_FDCAN_3; + else return BSP_FDCAN_ERR; +} + +static osMessageQueueId_t BSP_FDCAN_FindQueue(BSP_FDCAN_t fdcan, uint32_t can_id) { + BSP_FDCAN_QueueNode_t *node = queue_list; + while (node != NULL) { + if (node->fdcan == fdcan && node->can_id == can_id) return node->queue; + node = node->next; + } + return NULL; +} + +static int8_t BSP_FDCAN_CreateIdQueue(BSP_FDCAN_t fdcan, uint32_t can_id, uint8_t queue_size) { + if (queue_size == 0) queue_size = BSP_FDCAN_DEFAULT_QUEUE_SIZE; + if (osMutexAcquire(queue_mutex, FDCAN_QUEUE_MUTEX_TIMEOUT) != osOK) return BSP_ERR_TIMEOUT; + BSP_FDCAN_QueueNode_t *node = queue_list; + while (node != NULL) { + if (node->fdcan == fdcan && node->can_id == can_id) { + osMutexRelease(queue_mutex); + return BSP_ERR; + } + node = node->next; + } + + BSP_FDCAN_QueueNode_t *new_node = (BSP_FDCAN_QueueNode_t *)BSP_Malloc(sizeof(BSP_FDCAN_QueueNode_t)); + if (new_node == NULL) { osMutexRelease(queue_mutex); return BSP_ERR_NULL; } + new_node->queue = osMessageQueueNew(queue_size, sizeof(BSP_FDCAN_Message_t), NULL); + if (new_node->queue == NULL) { BSP_Free(new_node); osMutexRelease(queue_mutex); return BSP_ERR; } + new_node->fdcan = fdcan; + new_node->can_id = can_id; + new_node->queue_size = queue_size; + new_node->next = queue_list; + queue_list = new_node; + osMutexRelease(queue_mutex); + return BSP_OK; +} + +static BSP_FDCAN_FrameType_t BSP_FDCAN_GetFrameType(FDCAN_RxHeaderTypeDef *header) { + if (header->RxFrameType == FDCAN_REMOTE_FRAME) { + return (header->IdType == FDCAN_EXTENDED_ID) ? BSP_FDCAN_FRAME_EXT_REMOTE : BSP_FDCAN_FRAME_STD_REMOTE; + } else { + return (header->IdType == FDCAN_EXTENDED_ID) ? BSP_FDCAN_FRAME_EXT_DATA : BSP_FDCAN_FRAME_STD_DATA; + } +} + +static uint32_t BSP_FDCAN_DefaultIdParser(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type) { + (void)frame_type; + return original_id; +} + +static uint32_t BSP_FDCAN_EncodeDLC(uint8_t dlc) { + if (dlc <= 8) return dlc; + if (dlc <= 12) return FDCAN_DLC_BYTES_12; + if (dlc <= 16) return FDCAN_DLC_BYTES_16; + if (dlc <= 20) return FDCAN_DLC_BYTES_20; + if (dlc <= 24) return FDCAN_DLC_BYTES_24; + if (dlc <= 32) return FDCAN_DLC_BYTES_32; + if (dlc <= 48) return FDCAN_DLC_BYTES_48; + return FDCAN_DLC_BYTES_64; +} + +static void BSP_FDCAN_TxQueueInit(BSP_FDCAN_t fdcan) { + if (fdcan >= BSP_FDCAN_NUM) return; + tx_queues[fdcan].head = 0; + tx_queues[fdcan].tail = 0; +} + +static bool BSP_FDCAN_TxQueuePush(BSP_FDCAN_t fdcan, BSP_FDCAN_TxMessage_t *msg) { + if (fdcan >= BSP_FDCAN_NUM || msg == NULL) return false; + BSP_FDCAN_TxQueue_t *queue = &tx_queues[fdcan]; + uint32_t next_head = (queue->head + 1) % BSP_FDCAN_TX_QUEUE_SIZE; + if (next_head == queue->tail) return false; + queue->buffer[queue->head] = *msg; + queue->head = next_head; + return true; +} + +static bool BSP_FDCAN_TxQueuePop(BSP_FDCAN_t fdcan, BSP_FDCAN_TxMessage_t *msg) { + if (fdcan >= BSP_FDCAN_NUM || msg == NULL) return false; + BSP_FDCAN_TxQueue_t *queue = &tx_queues[fdcan]; + if (queue->head == queue->tail) return false; + *msg = queue->buffer[queue->tail]; + queue->tail = (queue->tail + 1) % BSP_FDCAN_TX_QUEUE_SIZE; + return true; +} + +static bool BSP_FDCAN_TxQueueIsEmpty(BSP_FDCAN_t fdcan) { + if (fdcan >= BSP_FDCAN_NUM) return true; + return tx_queues[fdcan].head == tx_queues[fdcan].tail; +} + +static void BSP_FDCAN_TxCompleteCallback(void) { + for (int i = 0; i < BSP_FDCAN_NUM; i++) { + BSP_FDCAN_t fdcan = (BSP_FDCAN_t)i; + FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle(fdcan); + if (hfdcan == NULL) continue; + // 消费所有 TX EVENT FIFO 事件，防止堵塞 + FDCAN_TxEventFifoTypeDef tx_event; + while (HAL_FDCAN_GetTxEvent(hfdcan, &tx_event) == HAL_OK) { + // 可在此统计 MessageMarker、ID、时间戳等 + } + // 续写软件队列到硬件 FIFO + BSP_FDCAN_TxMessage_t msg; + while (!BSP_FDCAN_TxQueueIsEmpty(fdcan)) { + if (HAL_FDCAN_GetTxFifoFreeLevel(hfdcan) == 0) break; + if (!BSP_FDCAN_TxQueuePop(fdcan, &msg)) break; + HAL_StatusTypeDef res = HAL_FDCAN_AddMessageToTxFifoQ(hfdcan, &msg.header, msg.data); + if (res != HAL_OK) { + break; + } + } + } +} + +static void BSP_FDCAN_RxFifo0Callback(void) { + FDCAN_RxHeaderTypeDef rx_header; + uint8_t rx_data[BSP_FDCAN_MAX_DLC]; + for (int fdcan_idx = 0; fdcan_idx < BSP_FDCAN_NUM; fdcan_idx++) { + FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle((BSP_FDCAN_t)fdcan_idx); + if (hfdcan == NULL) continue; + while (HAL_FDCAN_GetRxFifoFillLevel(hfdcan, FDCAN_RX_FIFO0) > 0) { + if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO0, &rx_header, rx_data) == HAL_OK) { + uint32_t original_id = (rx_header.IdType == FDCAN_STANDARD_ID) ? rx_header.Identifier&0x7ff : rx_header.Identifier&0x1fffffff; + BSP_FDCAN_FrameType_t frame_type = BSP_FDCAN_GetFrameType(&rx_header); + uint32_t parsed_id = BSP_FDCAN_ParseId(original_id, frame_type); + osMessageQueueId_t queue = BSP_FDCAN_FindQueue((BSP_FDCAN_t)fdcan_idx, parsed_id); + if (queue != NULL) { + BSP_FDCAN_Message_t msg; + msg.frame_type = frame_type; + msg.original_id = original_id; + msg.parsed_id = parsed_id; + uint8_t real_len = fdcan_dlc2len[rx_header.DataLength & 0xF]; + msg.dlc = real_len; + if (msg.dlc > BSP_FDCAN_MAX_DLC) msg.dlc = BSP_FDCAN_MAX_DLC; + memset(msg.data, 0, BSP_FDCAN_MAX_DLC);//现在是最大缓冲区写法所以全清零 + memcpy(msg.data, rx_data, msg.dlc); + osMessageQueuePut(queue, &msg, 0, 0); + } + } else { + break; + } + } + } +} + +static void BSP_FDCAN_RxFifo1Callback(void) { + FDCAN_RxHeaderTypeDef rx_header; + uint8_t rx_data[BSP_FDCAN_MAX_DLC]; + for (int fdcan_idx = 0; fdcan_idx < BSP_FDCAN_NUM; fdcan_idx++) { + FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle((BSP_FDCAN_t)fdcan_idx); + if (hfdcan == NULL) continue; + while (HAL_FDCAN_GetRxFifoFillLevel(hfdcan, FDCAN_RX_FIFO1) > 0) { + if (HAL_FDCAN_GetRxMessage(hfdcan, FDCAN_RX_FIFO1, &rx_header, rx_data) == HAL_OK) { + uint32_t original_id = (rx_header.IdType == FDCAN_STANDARD_ID) ? rx_header.Identifier&0x7ff : rx_header.Identifier&0x1fffffff; + BSP_FDCAN_FrameType_t frame_type = BSP_FDCAN_GetFrameType(&rx_header); + uint32_t parsed_id = BSP_FDCAN_ParseId(original_id, frame_type); + osMessageQueueId_t queue = BSP_FDCAN_FindQueue((BSP_FDCAN_t)fdcan_idx, parsed_id); + if (queue != NULL) { + BSP_FDCAN_Message_t msg; + msg.frame_type = frame_type; + msg.original_id = original_id; + msg.parsed_id = parsed_id; + uint8_t real_len = fdcan_dlc2len[rx_header.DataLength & 0xF]; + msg.dlc = real_len; + if (msg.dlc > BSP_FDCAN_MAX_DLC) msg.dlc = BSP_FDCAN_MAX_DLC; + memset(msg.data, 0, BSP_FDCAN_MAX_DLC);//现在是最大缓冲区写法所以全清零 + memcpy(msg.data, rx_data, msg.dlc); + osMessageQueuePut(queue, &msg, 0, 0); + } + } else { + break; + } + } + } +} + +/* HAL Callback Stubs (map HAL FDCAN callbacks to user callbacks) */ +void HAL_FDCAN_TxEventFifoCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t TxEventFifoITs) { + BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan); + if (bsp_fdcan != BSP_FDCAN_ERR) { + if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_EVENT_FIFO_CB]) + FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_EVENT_FIFO_CB](); + } +} + +void HAL_FDCAN_TxBufferCompleteCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndex) { + BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan); + if (bsp_fdcan != BSP_FDCAN_ERR) { + if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_BUFFER_COMPLETE_CB]) + FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_BUFFER_COMPLETE_CB](); + } +} + +void HAL_FDCAN_TxBufferAbortCallback(FDCAN_HandleTypeDef *hfdcan, uint32_t BufferIndex) { + BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan); + if (bsp_fdcan != BSP_FDCAN_ERR) { + if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_BUFFER_ABORT_CB]) + FDCAN_Callback[bsp_fdcan][HAL_FDCAN_TX_BUFFER_ABORT_CB](); + } +} + +void HAL_FDCAN_RxFifo0Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo0ITs) { + BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan); + if (bsp_fdcan != BSP_FDCAN_ERR) { + if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_RX_FIFO0_MSG_PENDING_CB]) + FDCAN_Callback[bsp_fdcan][HAL_FDCAN_RX_FIFO0_MSG_PENDING_CB](); + } +} + +void HAL_FDCAN_RxFifo1Callback(FDCAN_HandleTypeDef *hfdcan, uint32_t RxFifo1ITs) { + BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan); + if (bsp_fdcan != BSP_FDCAN_ERR) { + if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_RX_FIFO1_MSG_PENDING_CB]) + FDCAN_Callback[bsp_fdcan][HAL_FDCAN_RX_FIFO1_MSG_PENDING_CB](); + } +} + +void HAL_FDCAN_ErrorCallback(FDCAN_HandleTypeDef *hfdcan) { + BSP_FDCAN_t bsp_fdcan = FDCAN_Get(hfdcan); + if (bsp_fdcan != BSP_FDCAN_ERR) { + if (FDCAN_Callback[bsp_fdcan][HAL_FDCAN_ERROR_CB]) + FDCAN_Callback[bsp_fdcan][HAL_FDCAN_ERROR_CB](); + } +} + +/* Exported functions ------------------------------------------------------- */ +int8_t BSP_FDCAN_Init(void) { + if (inited) return BSP_ERR_INITED; + + memset(FDCAN_Callback, 0, sizeof(FDCAN_Callback)); + for (int i = 0; i < BSP_FDCAN_NUM; i++) BSP_FDCAN_TxQueueInit((BSP_FDCAN_t)i); + id_parser = BSP_FDCAN_DefaultIdParser; + queue_mutex = osMutexNew(NULL); + if (queue_mutex == NULL) return BSP_ERR; + + inited = true; + + /* 配置并启动 FDCAN 实例，绑定中断/回调 */ + + //========== 过滤器配置说明：========================== + // 过滤器编号：相对于每个（相当于经典can过滤器的bank） + // sFilterConfig.FilterIndex = 0 to 127(标准ID) or 0 to 63(扩展ID); + // 关于过滤器索引的说明： + // 由stm32h7xx_hal_fdcan.c的第1874行代码可知滤波器地址计算方式如下： + // StandardFilterSA（字节） = SRAMCAN_BASE + (MessageRAMOffset * 4U) + // 标准滤波器物理地址（字节） = StandardFilterSA + (FilterIndex * 4U)（每个标准滤波器占 4 字节 = 1 word,扩展的则是8个字节） + // + // + // 标识符类型: + // sFilterConfig.IdType = FDCAN_STANDARD_ID or FDCAN_EXTENDED_ID; + // 过滤器类型: （仅介绍掩码模式） + // sFilterConfig.FilterType = FDCAN_FILTER_MASK;(掩码模式） + // 过滤器配置: + // sFilterConfig.FilterConfig = FDCAN_FILTER_DISABLE; (禁用该过滤器条目) + // FDCAN_FILTER_TO_RXFIFO0; (将匹配的消息放入 FIFO 0（普通优先级）) + // FDCAN_FILTER_TO_RXFIFO1; (将匹配的消息放入 FIFO 1（高优先级）) + // FDCAN_FILTER_TO_RXBUFFER; (将匹配的消息放入 指定的接收缓冲区) + // FDCAN_FILTER_REJECT; (拒绝接收该标识符对应的报文) + // FDCAN_FILTER_ACCEPT; (接受所有消息) + // FDCAN_FILTER_HP (过滤器匹配时，将报文标记为高优先级) + // FDCAN_FILTER_TO_RXFIFO0_HP (过滤器匹配时，将报文标记为高优先级并存储至接收FIFO 0) + // FDCAN_FILTER_TO_RXFIFO1_HP (过滤器匹配时，将报文标记为高优先级并存储至接收FIFO 1) + // FDCAN_FILTER_TO_RXBUFFER (将报文存储至接收缓冲区，过滤器类型（FilterType）配置项失效 ) + // 过滤器ID与掩码(FilterType掩码模式下) + // 比较值（要匹配的 ID 的参考位） + // sFilterConfig.FilterID1 = 0 to 0x7FF; 标准ID + // 0 to 0x1FFFFFFF 扩展ID + // 掩码（1=比较该位，0=忽略该位） + // sFilterConfig.FilterID2 = 0 to 0x7FF; 标准ID + // 0 to 0x1FFFFFFF 扩展ID + // 接收缓冲区索引 + // FilterConfig == FDCAN_FILTER_TO_RXBUFFER 时有效;必须小于RxBuffersNbr配置的实际Rx buffer数量 + // sFilterConfig.RxBufferIndex = 0 to (RxBuffersNbr - 1); + // 标记校准信息（用于 FDCAN 校准/时钟相关单元作特殊处理或统计） + // 仅在FilterConfig 设为 FDCAN_FILTER_TO_RXBUFFER 时才有意义，通常设置为0 + // IsCalibrationMsg = 0 or 1; + // fdcan_filter_table.h + //================================================================================= + /* 依据上述说明，配置过滤器并启动FDCAN */ + FDCAN_FilterTypeDef sFilterConfig; + +#ifdef FDCAN1_EN + #define hfdcan hfdcan1 + #define FDCANX_RX_FIFO FDCAN1_RX_FIFO + FDCAN1_FILTER_CONFIG_TABLE(FDCAN_CONFIG_FILTER) + #undef hfdcan + #undef FDCANX_RX_FIFO + HAL_FDCAN_ConfigGlobalFilter(&hfdcan1, FDCAN1_GLOBAL_FILTER); + HAL_FDCAN_ActivateNotification(&hfdcan1, FDCANx_NOTIFY_FLAGS(FDCAN1_RX_FIFO), 0); + BSP_FDCAN_RegisterCallback(BSP_FDCAN_1, FDCANX_MSG_PENDING_CB(FDCAN1_RX_FIFO), BSP_FDCAN_RxFifo0Callback); + BSP_FDCAN_RegisterCallback(BSP_FDCAN_1, HAL_FDCAN_TX_EVENT_FIFO_CB, BSP_FDCAN_TxCompleteCallback); + HAL_FDCAN_Start(&hfdcan1); +#endif + +#ifdef FDCAN2_EN + #define hfdcan hfdcan2 + #define FDCANX_RX_FIFO FDCAN2_RX_FIFO + FDCAN2_FILTER_CONFIG_TABLE(FDCAN_CONFIG_FILTER) + #undef hfdcan + #undef FDCANX_RX_FIFO + HAL_FDCAN_ConfigGlobalFilter(&hfdcan2, FDCAN2_GLOBAL_FILTER); + HAL_FDCAN_ActivateNotification(&hfdcan2, FDCANx_NOTIFY_FLAGS(FDCAN2_RX_FIFO), 0); + BSP_FDCAN_RegisterCallback(BSP_FDCAN_2, FDCANX_MSG_PENDING_CB(FDCAN2_RX_FIFO), BSP_FDCAN_RxFifo1Callback); + BSP_FDCAN_RegisterCallback(BSP_FDCAN_2, HAL_FDCAN_TX_EVENT_FIFO_CB, BSP_FDCAN_TxCompleteCallback); + HAL_FDCAN_Start(&hfdcan2); +#endif + +#ifdef FDCAN3_EN + #define hfdcan hfdcan3 + #define FDCANX_RX_FIFO FDCAN3_RX_FIFO + FDCAN3_FILTER_CONFIG_TABLE(FDCAN_CONFIG_FILTER) + #undef hfdcan + #undef FDCANX_RX_FIFO + HAL_FDCAN_ConfigGlobalFilter(&hfdcan3, FDCAN3_GLOBAL_FILTER); + HAL_FDCAN_ActivateNotification(&hfdcan3, FDCANx_NOTIFY_FLAGS(FDCAN3_RX_FIFO), 0); + BSP_FDCAN_RegisterCallback(BSP_FDCAN_3, FDCANX_MSG_PENDING_CB(FDCAN3_RX_FIFO), BSP_FDCAN_RxFifo1Callback); + BSP_FDCAN_RegisterCallback(BSP_FDCAN_3, HAL_FDCAN_TX_EVENT_FIFO_CB, BSP_FDCAN_TxCompleteCallback); + HAL_FDCAN_Start(&hfdcan3); +#endif + +#undef FDCAN_FILTER_TO_RXFIFO_ENUM_INNER +#undef FDCAN_FILTER_TO_RXFIFO_ENUM +#undef FDCAN_CONFIG_FILTER +#undef FDCAN_NOTIFY_FLAG_RXFIFO_INNER +#undef FDCAN_NOTIFY_FLAG_RXFIFO +#undef FDCANx_NOTIFY_FLAGS +#undef FDCANX_MSG_PENDING_CB_INNER +#undef FDCANX_MSG_PENDING_CB + + return BSP_OK; +} + +FDCAN_HandleTypeDef *BSP_FDCAN_GetHandle(BSP_FDCAN_t fdcan) { + if (fdcan >= BSP_FDCAN_NUM) return NULL; + switch (fdcan) { + /* AUTO GENERATED BSP_FDCAN_GET_HANDLE BEGIN */ + case BSP_FDCAN_1: return &hfdcan1; + case BSP_FDCAN_2: return &hfdcan2; + case BSP_FDCAN_3: return &hfdcan3; + /* AUTO GENERATED BSP_FDCAN_GET_HANDLE END */ + default: return NULL; + } +} + +int8_t BSP_FDCAN_RegisterCallback(BSP_FDCAN_t fdcan, BSP_FDCAN_Callback_t type, void (*callback)(void)) { + if (!inited) return BSP_ERR_INITED; + if (callback == NULL) return BSP_ERR_NULL; + if (fdcan >= BSP_FDCAN_NUM) return BSP_ERR; + if (type >= HAL_FDCAN_CB_NUM) return BSP_ERR; + FDCAN_Callback[fdcan][type] = callback; + return BSP_OK; +} + +int8_t BSP_FDCAN_Transmit(BSP_FDCAN_t fdcan, BSP_FDCAN_Format_t format, uint32_t id, uint8_t *data, uint8_t dlc) { + if (!inited) return BSP_ERR_INITED; + if (fdcan >= BSP_FDCAN_NUM) return BSP_ERR; + if (data == NULL && format != BSP_FDCAN_FORMAT_STD_REMOTE && format != BSP_FDCAN_FORMAT_EXT_REMOTE) return BSP_ERR_NULL; + if (dlc > BSP_FDCAN_MAX_DLC) return BSP_ERR; + FDCAN_HandleTypeDef *hfdcan = BSP_FDCAN_GetHandle(fdcan); + if (hfdcan == NULL) return BSP_ERR_NULL; + + BSP_FDCAN_TxMessage_t tx_msg = {0}; + switch (format) { + case BSP_FDCAN_FORMAT_STD_DATA: + tx_msg.header.Identifier = id; + tx_msg.header.IdType = FDCAN_STANDARD_ID; + tx_msg.header.TxFrameType = FDCAN_DATA_FRAME; + break; + case BSP_FDCAN_FORMAT_EXT_DATA: + tx_msg.header.Identifier = id; + tx_msg.header.IdType = FDCAN_EXTENDED_ID; + tx_msg.header.TxFrameType = FDCAN_DATA_FRAME; + break; + case BSP_FDCAN_FORMAT_STD_REMOTE: + tx_msg.header.Identifier = id; + tx_msg.header.IdType = FDCAN_STANDARD_ID; + tx_msg.header.TxFrameType = FDCAN_REMOTE_FRAME; + break; + case BSP_FDCAN_FORMAT_EXT_REMOTE: + tx_msg.header.Identifier = id; + tx_msg.header.IdType = FDCAN_EXTENDED_ID; + tx_msg.header.TxFrameType = FDCAN_REMOTE_FRAME; + break; + default: + return BSP_ERR; + } + switch (hfdcan->Init.FrameFormat) { + case FDCAN_FRAME_FD_BRS: + tx_msg.header.BitRateSwitch = FDCAN_BRS_ON; + tx_msg.header.FDFormat = FDCAN_FD_CAN; + break; + case FDCAN_FRAME_FD_NO_BRS: + tx_msg.header.BitRateSwitch = FDCAN_BRS_OFF; + tx_msg.header.FDFormat = FDCAN_FD_CAN; + break; + case FDCAN_FRAME_CLASSIC: + default: + tx_msg.header.BitRateSwitch = FDCAN_BRS_OFF; + tx_msg.header.FDFormat = FDCAN_CLASSIC_CAN; + break; + } + tx_msg.header.ErrorStateIndicator = FDCAN_ESI_ACTIVE; + tx_msg.header.TxEventFifoControl = FDCAN_STORE_TX_EVENTS; + tx_msg.header.MessageMarker = 0x01; + tx_msg.header.DataLength = BSP_FDCAN_EncodeDLC(dlc); + + memset(tx_msg.data, 0, dlc); + if (data != NULL && dlc > 0) {memcpy(tx_msg.data, data, dlc);} + + if (HAL_FDCAN_GetTxFifoFreeLevel(hfdcan) > 0) { + if (HAL_FDCAN_AddMessageToTxFifoQ(hfdcan, &tx_msg.header, tx_msg.data) == HAL_OK) return BSP_OK; + } + if (BSP_FDCAN_TxQueuePush(fdcan, &tx_msg)) return BSP_OK; + return BSP_ERR; +} + +int8_t BSP_FDCAN_TransmitStdDataFrame(BSP_FDCAN_t fdcan, BSP_FDCAN_StdDataFrame_t *frame) { + if (frame == NULL) return BSP_ERR_NULL; + return BSP_FDCAN_Transmit(fdcan, BSP_FDCAN_FORMAT_STD_DATA, frame->id, frame->data, frame->dlc); +} + +int8_t BSP_FDCAN_TransmitExtDataFrame(BSP_FDCAN_t fdcan, BSP_FDCAN_ExtDataFrame_t *frame) { + if (frame == NULL) return BSP_ERR_NULL; + return BSP_FDCAN_Transmit(fdcan, BSP_FDCAN_FORMAT_EXT_DATA, frame->id, frame->data, frame->dlc); +} + +int8_t BSP_FDCAN_TransmitRemoteFrame(BSP_FDCAN_t fdcan, BSP_FDCAN_RemoteFrame_t *frame) { + if (frame == NULL) return BSP_ERR_NULL; + BSP_FDCAN_Format_t format = frame->is_extended ? BSP_FDCAN_FORMAT_EXT_REMOTE : BSP_FDCAN_FORMAT_STD_REMOTE; + return BSP_FDCAN_Transmit(fdcan, format, frame->id, NULL, frame->dlc); +} + +int8_t BSP_FDCAN_RegisterId(BSP_FDCAN_t fdcan, uint32_t can_id, uint8_t queue_size) { + if (!inited) return BSP_ERR_INITED; + return BSP_FDCAN_CreateIdQueue(fdcan, can_id, queue_size); +} + +int8_t BSP_FDCAN_GetMessage(BSP_FDCAN_t fdcan, uint32_t can_id, BSP_FDCAN_Message_t *msg, uint32_t timeout) { + if (!inited) return BSP_ERR_INITED; + if (msg == NULL) return BSP_ERR_NULL; + if (osMutexAcquire(queue_mutex, FDCAN_QUEUE_MUTEX_TIMEOUT) != osOK) return BSP_ERR_TIMEOUT; + osMessageQueueId_t queue = BSP_FDCAN_FindQueue(fdcan, can_id); + osMutexRelease(queue_mutex); + if (queue == NULL) return BSP_ERR_NO_DEV; + osStatus_t res = osMessageQueueGet(queue, msg, NULL, timeout); + return (res == osOK) ? BSP_OK : BSP_ERR; +} + +int32_t BSP_FDCAN_GetQueueCount(BSP_FDCAN_t fdcan, uint32_t can_id) { + if (!inited) return -1; + if (osMutexAcquire(queue_mutex, FDCAN_QUEUE_MUTEX_TIMEOUT) != osOK) return -1; + osMessageQueueId_t queue = BSP_FDCAN_FindQueue(fdcan, can_id); + osMutexRelease(queue_mutex); + if (queue == NULL) return -1; + return (int32_t)osMessageQueueGetCount(queue); +} + +int8_t BSP_FDCAN_FlushQueue(BSP_FDCAN_t fdcan, uint32_t can_id) { + if (!inited) return BSP_ERR_INITED; + if (osMutexAcquire(queue_mutex, FDCAN_QUEUE_MUTEX_TIMEOUT) != osOK) return BSP_ERR_TIMEOUT; + osMessageQueueId_t queue = BSP_FDCAN_FindQueue(fdcan, can_id); + osMutexRelease(queue_mutex); + if (queue == NULL) return BSP_ERR_NO_DEV; + BSP_FDCAN_Message_t tmp; + while (osMessageQueueGet(queue, &tmp, NULL, BSP_FDCAN_TIMEOUT_IMMEDIATE) == osOK) { } + return BSP_OK; +} + +int8_t BSP_FDCAN_RegisterIdParser(BSP_FDCAN_IdParser_t parser) { + if (!inited) return BSP_ERR_INITED; + if (parser == NULL) return BSP_ERR_NULL; + id_parser = parser; + return BSP_OK; +} + +uint32_t BSP_FDCAN_ParseId(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type) { + if (id_parser != NULL) return id_parser(original_id, frame_type); + return BSP_FDCAN_DefaultIdParser(original_id, frame_type); +} +/* */ + diff --git a/User/bsp/fdcan.h b/User/bsp/fdcan.h new file mode 100644 index 0000000..99ecd80 --- /dev/null +++ b/User/bsp/fdcan.h @@ -0,0 +1,137 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include "bsp/bsp.h" +#include "bsp/mm.h" +#include + +/* USER INCLUDE BEGIN */ +#include +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +#define BSP_FDCAN_MAX_DLC 64 +#define BSP_FDCAN_DEFAULT_QUEUE_SIZE 10 +#define BSP_FDCAN_TIMEOUT_IMMEDIATE 0 +#define BSP_FDCAN_TIMEOUT_FOREVER osWaitForever +#define BSP_FDCAN_TX_QUEUE_SIZE 32 +/* Exported macro ----------------------------------------------------------- */ +//FDCANX实例使能 +/* AUTO GENERATED FDCAN_EN BEGIN */ +#define FDCAN1_EN +#define FDCAN2_EN +#define FDCAN3_EN +/* AUTO GENERATED FDCAN_EN END */ + +// FDCANX接收FIFO选择（0=FIFO0, 1=FIFO1） +/* AUTO GENERATED FDCAN_RX_FIFO BEGIN */ +#ifdef FDCAN1_EN + #define FDCAN1_RX_FIFO 0 +#endif +#ifdef FDCAN2_EN + #define FDCAN2_RX_FIFO 1 +#endif +#ifdef FDCAN3_EN + #define FDCAN3_RX_FIFO 1 +#endif +/* AUTO GENERATED FDCAN_RX_FIFO END */ +/* Exported types ----------------------------------------------------------- */ +typedef enum { + /* AUTO GENERATED BSP_FDCAN_NAME BEGIN */ + BSP_FDCAN_1, + BSP_FDCAN_2, + BSP_FDCAN_3, + /* AUTO GENERATED BSP_FDCAN_NAME END */ + BSP_FDCAN_NUM, + BSP_FDCAN_ERR, +} BSP_FDCAN_t; + +typedef enum { + HAL_FDCAN_TX_EVENT_FIFO_CB, + HAL_FDCAN_TX_BUFFER_COMPLETE_CB, + HAL_FDCAN_TX_BUFFER_ABORT_CB, + HAL_FDCAN_RX_FIFO0_MSG_PENDING_CB, + HAL_FDCAN_RX_FIFO0_FULL_CB, + HAL_FDCAN_RX_FIFO1_MSG_PENDING_CB, + HAL_FDCAN_RX_FIFO1_FULL_CB, + HAL_FDCAN_ERROR_CB, + HAL_FDCAN_CB_NUM, +} BSP_FDCAN_Callback_t; + +typedef enum { + BSP_FDCAN_FORMAT_STD_DATA, + BSP_FDCAN_FORMAT_EXT_DATA, + BSP_FDCAN_FORMAT_STD_REMOTE, + BSP_FDCAN_FORMAT_EXT_REMOTE, +} BSP_FDCAN_Format_t; + +typedef enum { + BSP_FDCAN_FRAME_STD_DATA, + BSP_FDCAN_FRAME_EXT_DATA, + BSP_FDCAN_FRAME_STD_REMOTE, + BSP_FDCAN_FRAME_EXT_REMOTE, +} BSP_FDCAN_FrameType_t; + +typedef struct { + BSP_FDCAN_FrameType_t frame_type; + uint32_t original_id; + uint32_t parsed_id; + uint8_t dlc; + uint8_t data[BSP_FDCAN_MAX_DLC]; + uint32_t timestamp; +} BSP_FDCAN_Message_t; + +typedef struct { + uint32_t id; + uint8_t dlc; + uint8_t data[BSP_FDCAN_MAX_DLC]; +} BSP_FDCAN_StdDataFrame_t; + +typedef struct { + uint32_t id; + uint8_t dlc; + uint8_t data[BSP_FDCAN_MAX_DLC]; +} BSP_FDCAN_ExtDataFrame_t; + +typedef struct { + uint32_t id; + uint8_t dlc; + bool is_extended; +} BSP_FDCAN_RemoteFrame_t; + +typedef uint32_t (*BSP_FDCAN_IdParser_t)(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type); + +typedef struct { + FDCAN_TxHeaderTypeDef header; /* HAL FDCAN header type */ + uint8_t data[BSP_FDCAN_MAX_DLC]; +} BSP_FDCAN_TxMessage_t; + +typedef struct { + BSP_FDCAN_TxMessage_t buffer[BSP_FDCAN_TX_QUEUE_SIZE]; + volatile uint32_t head; + volatile uint32_t tail; +} BSP_FDCAN_TxQueue_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BSP_FDCAN_Init(void); +FDCAN_HandleTypeDef *BSP_FDCAN_GetHandle(BSP_FDCAN_t can); +int8_t BSP_FDCAN_RegisterCallback(BSP_FDCAN_t can, BSP_FDCAN_Callback_t type, void (*callback)(void)); +int8_t BSP_FDCAN_Transmit(BSP_FDCAN_t can, BSP_FDCAN_Format_t format, uint32_t id, uint8_t *data, uint8_t dlc); +int8_t BSP_FDCAN_TransmitStdDataFrame(BSP_FDCAN_t can, BSP_FDCAN_StdDataFrame_t *frame); +int8_t BSP_FDCAN_TransmitExtDataFrame(BSP_FDCAN_t can, BSP_FDCAN_ExtDataFrame_t *frame); +int8_t BSP_FDCAN_TransmitRemoteFrame(BSP_FDCAN_t can, BSP_FDCAN_RemoteFrame_t *frame); +int8_t BSP_FDCAN_RegisterId(BSP_FDCAN_t can, uint32_t can_id, uint8_t queue_size); +int8_t BSP_FDCAN_GetMessage(BSP_FDCAN_t can, uint32_t can_id, BSP_FDCAN_Message_t *msg, uint32_t timeout); +int32_t BSP_FDCAN_GetQueueCount(BSP_FDCAN_t can, uint32_t can_id); +int8_t BSP_FDCAN_FlushQueue(BSP_FDCAN_t can, uint32_t can_id); +int8_t BSP_FDCAN_RegisterIdParser(BSP_FDCAN_IdParser_t parser); +uint32_t BSP_FDCAN_ParseId(uint32_t original_id, BSP_FDCAN_FrameType_t frame_type); +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/flash.c b/User/bsp/flash.c new file mode 100644 index 0000000..0771a1f --- /dev/null +++ b/User/bsp/flash.c @@ -0,0 +1,82 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/flash.h" + +#include +#include + +/* Private define ----------------------------------------------------------- */ +/* USER CODE BEGIN FLASH_MAX_SECTOR */ +#define BSP_FLASH_MAX_SECTOR 7 +/* USER CODE END FLASH_MAX_SECTOR */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +void BSP_Flash_EraseSector(uint32_t sector) { + FLASH_EraseInitTypeDef flash_erase; + uint32_t sector_error; + + /* USER CODE BEGIN FLASH_ERASE_CHECK */ + if (sector > 0 && sector <= 7) { + /* USER CODE END FLASH_ERASE_CHECK */ + flash_erase.Sector = sector; + flash_erase.TypeErase = FLASH_TYPEERASE_SECTORS; + flash_erase.VoltageRange = FLASH_VOLTAGE_RANGE_3; + flash_erase.NbSectors = 1; +#if defined(STM32H7) + flash_erase.Banks = FLASH_BANK_1; // H7 requires Bank parameter +#endif + + HAL_FLASH_Unlock(); +#if defined(STM32H7) + while (FLASH_WaitForLastOperation(50, FLASH_BANK_1) != HAL_OK) + ; +#else + while (FLASH_WaitForLastOperation(50) != HAL_OK) + ; +#endif + HAL_FLASHEx_Erase(&flash_erase, §or_error); + HAL_FLASH_Lock(); + } + /* USER CODE BEGIN FLASH_ERASE_END */ + /* USER CODE END FLASH_ERASE_END */ +} + +void BSP_Flash_WriteBytes(uint32_t address, const uint8_t *buf, size_t len) { + HAL_FLASH_Unlock(); +#if defined(STM32H7) + // H7 uses FLASHWORD (32 bytes) programming + uint8_t flash_word[32] __attribute__((aligned(32))); + while (len > 0) { + size_t chunk = (len < 32) ? len : 32; + memset(flash_word, 0xFF, 32); + memcpy(flash_word, buf, chunk); + + while (FLASH_WaitForLastOperation(50, FLASH_BANK_1) != HAL_OK) + ; + HAL_FLASH_Program(FLASH_TYPEPROGRAM_FLASHWORD, address, (uint32_t)flash_word); + + address += 32; + buf += chunk; + len -= chunk; + } +#else + // F4/F7 use byte programming + while (len > 0) { + while (FLASH_WaitForLastOperation(50) != HAL_OK) + ; + HAL_FLASH_Program(FLASH_TYPEPROGRAM_BYTE, address, *buf); + address++; + buf++; + len--; + } +#endif + HAL_FLASH_Lock(); +} + +void BSP_Flash_ReadBytes(uint32_t address, void *buf, size_t len) { + memcpy(buf, (void *)address, len); +} diff --git a/User/bsp/flash.h b/User/bsp/flash.h new file mode 100644 index 0000000..e95f18f --- /dev/null +++ b/User/bsp/flash.h @@ -0,0 +1,46 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ------------------------------------------------------------------ */ +#include + +#include "bsp/bsp.h" + +/* Exported constants -------------------------------------------------------- */ +/* Base address of the Flash sectors */ +/* USER CODE BEGIN FLASH_SECTOR_DEFINES */ +#define ADDR_FLASH_SECTOR_0 ((uint32_t)0x08000000) +/* Base address of Sector 0, 128 Kbytes */ +#define ADDR_FLASH_SECTOR_1 ((uint32_t)0x08020000) +/* Base address of Sector 1, 128 Kbytes */ +#define ADDR_FLASH_SECTOR_2 ((uint32_t)0x08040000) +/* Base address of Sector 2, 128 Kbytes */ +#define ADDR_FLASH_SECTOR_3 ((uint32_t)0x08060000) +/* Base address of Sector 3, 128 Kbytes */ +#define ADDR_FLASH_SECTOR_4 ((uint32_t)0x08080000) +/* Base address of Sector 4, 128 Kbytes */ +#define ADDR_FLASH_SECTOR_5 ((uint32_t)0x080A0000) +/* Base address of Sector 5, 128 Kbytes */ +#define ADDR_FLASH_SECTOR_6 ((uint32_t)0x080C0000) +/* Base address of Sector 6, 128 Kbytes */ +#define ADDR_FLASH_SECTOR_7 ((uint32_t)0x080E0000) +/* Base address of Sector 7, 128 Kbytes */ +/* USER CODE END FLASH_SECTOR_DEFINES */ + +/* USER CODE BEGIN FLASH_END_ADDRESS */ +#define ADDR_FLASH_END ((uint32_t)0x08100000) /* End address for flash */ +/* USER CODE END FLASH_END_ADDRESS */ + +/* Exported macro ------------------------------------------------------------ */ +/* Exported types ------------------------------------------------------------ */ +/* Exported functions prototypes --------------------------------------------- */ +void BSP_Flash_EraseSector(uint32_t sector); +void BSP_Flash_WriteBytes(uint32_t address, const uint8_t *buf, size_t len); +void BSP_Flash_ReadBytes(uint32_t address, void *buf, size_t len); + +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/gpio.c b/User/bsp/gpio.c new file mode 100644 index 0000000..9be5893 --- /dev/null +++ b/User/bsp/gpio.c @@ -0,0 +1,123 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/gpio.h" + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct { + uint16_t pin; + GPIO_TypeDef *gpio; +} BSP_GPIO_MAP_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static const BSP_GPIO_MAP_t GPIO_Map[BSP_GPIO_NUM] = { + {LCD_BLK_Pin, LCD_BLK_GPIO_Port}, + {LCD_RES_Pin, LCD_RES_GPIO_Port}, + {ACC_CS_Pin, ACC_CS_GPIO_Port}, + {POWER_24V_2_Pin, POWER_24V_2_GPIO_Port}, + {POWER_5V_Pin, POWER_5V_GPIO_Port}, + {GYRO_CS_Pin, GYRO_CS_GPIO_Port}, + {ACC_INT_Pin, ACC_INT_GPIO_Port}, + {W25Q64_CS_Pin, W25Q64_CS_GPIO_Port}, + {GYRO_INT_Pin, GYRO_INT_GPIO_Port}, + {LCD_CS_Pin, LCD_CS_GPIO_Port}, +}; + +static void (*GPIO_Callback[16])(void); + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin) { + for (uint8_t i = 0; i < 16; i++) { + if (GPIO_Pin & (1 << i)) { + if (GPIO_Callback[i]) { + GPIO_Callback[i](); + } + } + } +} + +/* Exported functions ------------------------------------------------------- */ +int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + + // 从GPIO映射中获取对应的pin值 + uint16_t pin = GPIO_Map[gpio].pin; + + for (uint8_t i = 0; i < 16; i++) { + if (pin & (1 << i)) { + GPIO_Callback[i] = callback; + break; + } + } + return BSP_OK; +} + +int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio) { + switch (gpio) { + case BSP_GPIO_ACC_INT: +#if defined(ACC_INT_EXTI_IRQn) + HAL_NVIC_EnableIRQ(ACC_INT_EXTI_IRQn); +#endif + return BSP_OK; + case BSP_GPIO_GYRO_INT: +#if defined(GYRO_INT_EXTI_IRQn) + HAL_NVIC_EnableIRQ(GYRO_INT_EXTI_IRQn); +#endif + return BSP_OK; + default: + return BSP_ERR; + } +} + +int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio) { + switch (gpio) { + case BSP_GPIO_ACC_INT: +#if defined(ACC_INT_EXTI_IRQn) + HAL_NVIC_DisableIRQ(ACC_INT_EXTI_IRQn); +#endif + return BSP_OK; + case BSP_GPIO_GYRO_INT: +#if defined(GYRO_INT_EXTI_IRQn) + HAL_NVIC_DisableIRQ(GYRO_INT_EXTI_IRQn); +#endif + return BSP_OK; + default: + return BSP_ERR; + } +} +int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value){ + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + HAL_GPIO_WritePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin, value); + return BSP_OK; +} + +int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio){ + if (gpio >= BSP_GPIO_NUM) return BSP_ERR; + HAL_GPIO_TogglePin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin); + return BSP_OK; +} + +bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio){ + if (gpio >= BSP_GPIO_NUM) return false; + return HAL_GPIO_ReadPin(GPIO_Map[gpio].gpio, GPIO_Map[gpio].pin) == GPIO_PIN_SET; +} \ No newline at end of file diff --git a/User/bsp/gpio.h b/User/bsp/gpio.h new file mode 100644 index 0000000..c2fe4ad --- /dev/null +++ b/User/bsp/gpio.h @@ -0,0 +1,56 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +typedef enum { + BSP_GPIO_LCD_BLK, + BSP_GPIO_LCD_RES, + BSP_GPIO_ACC_CS, + BSP_GPIO_POWER_24V_2, + BSP_GPIO_POWER_5V, + BSP_GPIO_GYRO_CS, + BSP_GPIO_ACC_INT, + BSP_GPIO_W25Q64_CS, + BSP_GPIO_GYRO_INT, + BSP_GPIO_LCD_CS, + BSP_GPIO_NUM, + BSP_GPIO_ERR, +} BSP_GPIO_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BSP_GPIO_RegisterCallback(BSP_GPIO_t gpio, void (*callback)(void)); + +int8_t BSP_GPIO_EnableIRQ(BSP_GPIO_t gpio); +int8_t BSP_GPIO_DisableIRQ(BSP_GPIO_t gpio); + +int8_t BSP_GPIO_WritePin(BSP_GPIO_t gpio, bool value); +int8_t BSP_GPIO_TogglePin(BSP_GPIO_t gpio); + +bool BSP_GPIO_ReadPin(BSP_GPIO_t gpio); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/bsp/mm.c b/User/bsp/mm.c new file mode 100644 index 0000000..13d20c0 --- /dev/null +++ b/User/bsp/mm.c @@ -0,0 +1,30 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/mm.h" + +#include "FreeRTOS.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +inline void *BSP_Malloc(size_t size) { return pvPortMalloc(size); } + +inline void BSP_Free(void *pv) { vPortFree(pv); } + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/bsp/mm.h b/User/bsp/mm.h new file mode 100644 index 0000000..d24634e --- /dev/null +++ b/User/bsp/mm.h @@ -0,0 +1,32 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +/* Exported functions prototypes -------------------------------------------- */ +void *BSP_Malloc(size_t size); +void BSP_Free(void *pv); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/pwm.c b/User/bsp/pwm.c new file mode 100644 index 0000000..9d3c275 --- /dev/null +++ b/User/bsp/pwm.c @@ -0,0 +1,115 @@ +/* Includes ----------------------------------------------------------------- */ +#include "tim.h" +#include "bsp/pwm.h" +#include "bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +typedef struct { + TIM_HandleTypeDef *tim; + uint16_t channel; +} BSP_PWM_Config_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static const BSP_PWM_Config_t PWM_Map[BSP_PWM_NUM] = { + {&htim2, TIM_CHANNEL_1}, + {&htim2, TIM_CHANNEL_3}, + {&htim3, TIM_CHANNEL_4}, + {&htim12, TIM_CHANNEL_2}, + {&htim1, TIM_CHANNEL_3}, + {&htim1, TIM_CHANNEL_1}, +}; + +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Start(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} + +int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + if (duty_cycle > 1.0f) { + duty_cycle = 1.0f; + } + if (duty_cycle < 0.0f) { + duty_cycle = 0.0f; + } + // 获取ARR值（周期值） + uint32_t arr = __HAL_TIM_GET_AUTORELOAD(PWM_Map[ch].tim); + + // 计算比较值：CCR = duty_cycle * (ARR + 1) + uint32_t ccr = (uint32_t)(duty_cycle * (arr + 1)); + + __HAL_TIM_SET_COMPARE(PWM_Map[ch].tim, PWM_Map[ch].channel, ccr); + + return BSP_OK; +} + +int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + uint32_t timer_clock = HAL_RCC_GetPCLK1Freq(); // Get the timer clock frequency + uint32_t prescaler = PWM_Map[ch].tim->Init.Prescaler; + uint32_t period = (timer_clock / (prescaler + 1)) / freq - 1; + + if (period > UINT16_MAX) { + return BSP_ERR; // Frequency too low + } + __HAL_TIM_SET_AUTORELOAD(PWM_Map[ch].tim, period); + + return BSP_OK; +} + +int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Stop(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} + +uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + return PWM_Map[ch].tim->Init.AutoReloadPreload; +} + +TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch) { + return PWM_Map[ch].tim; +} + + +uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + return PWM_Map[ch].channel; +} + +int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Start_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel, pData, Length); + return BSP_OK; +} + +int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch) { + if (ch >= BSP_PWM_NUM) return BSP_ERR; + + HAL_TIM_PWM_Stop_DMA(PWM_Map[ch].tim, PWM_Map[ch].channel); + return BSP_OK; +} \ No newline at end of file diff --git a/User/bsp/pwm.h b/User/bsp/pwm.h new file mode 100644 index 0000000..f0e75f2 --- /dev/null +++ b/User/bsp/pwm.h @@ -0,0 +1,53 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include "tim.h" +#include "bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +/* PWM通道 */ +typedef enum { + BSP_PWM_TIM2_CH1, + BSP_PWM_TIM2_CH3, + BSP_PWM_TIM3_CH4, + BSP_PWM_TIM12_CH2, + BSP_PWM_TIM1_CH3, + BSP_PWM_TIM1_CH1, + BSP_PWM_NUM, + BSP_PWM_ERR, +} BSP_PWM_Channel_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BSP_PWM_Start(BSP_PWM_Channel_t ch); +int8_t BSP_PWM_SetComp(BSP_PWM_Channel_t ch, float duty_cycle); +int8_t BSP_PWM_SetFreq(BSP_PWM_Channel_t ch, float freq); +int8_t BSP_PWM_Stop(BSP_PWM_Channel_t ch); +uint32_t BSP_PWM_GetAutoReloadPreload(BSP_PWM_Channel_t ch); +uint16_t BSP_PWM_GetChannel(BSP_PWM_Channel_t ch); +TIM_HandleTypeDef* BSP_PWM_GetHandle(BSP_PWM_Channel_t ch); +int8_t BSP_PWM_Start_DMA(BSP_PWM_Channel_t ch, uint32_t *pData, uint16_t Length); +int8_t BSP_PWM_Stop_DMA(BSP_PWM_Channel_t ch); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif \ No newline at end of file diff --git a/User/bsp/spi.c b/User/bsp/spi.c new file mode 100644 index 0000000..0727529 --- /dev/null +++ b/User/bsp/spi.c @@ -0,0 +1,181 @@ +/* Includes ----------------------------------------------------------------- */ +#include +#include "bsp/spi.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static void (*SPI_Callback[BSP_SPI_NUM][BSP_SPI_CB_NUM])(void); + +/* Private function -------------------------------------------------------- */ +static BSP_SPI_t SPI_Get(SPI_HandleTypeDef *hspi) { + if (hspi->Instance == SPI2) + return BSP_SPI_BMI088; + else + return BSP_SPI_ERR; +} + +void HAL_SPI_TxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB]) { + SPI_Callback[bsp_spi][BSP_SPI_TX_CPLT_CB](); + } + } +} + +void HAL_SPI_RxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_CPLT_CB](); + } +} + +void HAL_SPI_TxRxCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_CPLT_CB](); + } +} + +void HAL_SPI_TxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_RxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_RX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_TxRxHalfCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_TX_RX_HALF_CPLT_CB](); + } +} + +void HAL_SPI_ErrorCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_ERROR_CB](); + } +} + +void HAL_SPI_AbortCpltCallback(SPI_HandleTypeDef *hspi) { + BSP_SPI_t bsp_spi = SPI_Get(hspi); + if (bsp_spi != BSP_SPI_ERR) { + if (SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB]) + SPI_Callback[SPI_Get(hspi)][BSP_SPI_ABORT_CPLT_CB](); + } +} + +/* Exported functions ------------------------------------------------------- */ +SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi) { + switch (spi) { + case BSP_SPI_BMI088: + return &hspi2; + default: + return NULL; + } +} + +int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type, + void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + SPI_Callback[spi][type] = callback; + return BSP_OK; +} + +int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_Transmit_DMA(hspi, data, size)!= HAL_OK;; + } else { + return HAL_SPI_Transmit(hspi, data, size, 20)!= HAL_OK;; + } +} + +int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_Receive_DMA(hspi, data, size)!= HAL_OK;; + } else { + return HAL_SPI_Receive(hspi, data, size, 20)!= HAL_OK;; + } +} + +int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData, + uint16_t size, bool dma) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + SPI_HandleTypeDef *hspi = BSP_SPI_GetHandle(spi); + if (hspi == NULL) return BSP_ERR; + + if (dma) { + return HAL_SPI_TransmitReceive_DMA(hspi, txData, rxData, size)!= HAL_OK;; + } else { + return HAL_SPI_TransmitReceive(hspi, txData, rxData, size, 20)!= HAL_OK;; + } +} + +uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg) { + if (spi >= BSP_SPI_NUM) return 0xFF; + uint8_t tmp[2] = {reg | 0x80, 0x00}; + BSP_SPI_TransmitReceive(spi, tmp, tmp, 2u, true); + return tmp[1]; +} + +int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + uint8_t tmp[2] = {reg & 0x7f, data}; + return BSP_SPI_Transmit(spi, tmp, 2u, true); +} + +int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + reg = reg | 0x80; + BSP_SPI_Transmit(spi, ®, 1u, true); + return BSP_SPI_Receive(spi, data, size, true); +} + +int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size) { + if (spi >= BSP_SPI_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + reg = reg & 0x7f; + BSP_SPI_Transmit(spi, ®, 1u, true); + return BSP_SPI_Transmit(spi, data, size, true); +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/bsp/spi.h b/User/bsp/spi.h new file mode 100644 index 0000000..2abb0c6 --- /dev/null +++ b/User/bsp/spi.h @@ -0,0 +1,70 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ + +/* 要添加使用SPI的新设备，需要先在此添加对应的枚举值 */ + +/* SPI实体枚举，与设备对应 */ +typedef enum { + BSP_SPI_BMI088, + BSP_SPI_NUM, + BSP_SPI_ERR, +} BSP_SPI_t; + +/* SPI支持的中断回调函数类型，具体参考HAL中定义 */ +typedef enum { + BSP_SPI_TX_CPLT_CB, + BSP_SPI_RX_CPLT_CB, + BSP_SPI_TX_RX_CPLT_CB, + BSP_SPI_TX_HALF_CPLT_CB, + BSP_SPI_RX_HALF_CPLT_CB, + BSP_SPI_TX_RX_HALF_CPLT_CB, + BSP_SPI_ERROR_CB, + BSP_SPI_ABORT_CPLT_CB, + BSP_SPI_CB_NUM, +} BSP_SPI_Callback_t; + +/* Exported functions prototypes -------------------------------------------- */ +SPI_HandleTypeDef *BSP_SPI_GetHandle(BSP_SPI_t spi); +int8_t BSP_SPI_RegisterCallback(BSP_SPI_t spi, BSP_SPI_Callback_t type, + void (*callback)(void)); + + +int8_t BSP_SPI_Transmit(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_SPI_Receive(BSP_SPI_t spi, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_SPI_TransmitReceive(BSP_SPI_t spi, uint8_t *txData, uint8_t *rxData, + uint16_t size, bool dma); + +uint8_t BSP_SPI_MemReadByte(BSP_SPI_t spi, uint8_t reg); +int8_t BSP_SPI_MemWriteByte(BSP_SPI_t spi, uint8_t reg, uint8_t data); +int8_t BSP_SPI_MemRead(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size); +int8_t BSP_SPI_MemWrite(BSP_SPI_t spi, uint8_t reg, uint8_t *data, uint16_t size); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/time.c b/User/bsp/time.c new file mode 100644 index 0000000..21918ed --- /dev/null +++ b/User/bsp/time.c @@ -0,0 +1,81 @@ +/* Includes ----------------------------------------------------------------- */ +#include "bsp/time.h" +#include "bsp.h" + +#include +#include "FreeRTOS.h" +#include "main.h" +#include "task.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +/* Private function -------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +uint32_t BSP_TIME_Get_ms() { return xTaskGetTickCount(); } + +uint64_t BSP_TIME_Get_us() { + uint32_t tick_freq = osKernelGetTickFreq(); + uint32_t ticks_old = xTaskGetTickCount()*(1000/tick_freq); + uint32_t tick_value_old = SysTick->VAL; + uint32_t ticks_new = xTaskGetTickCount()*(1000/tick_freq); + uint32_t tick_value_new = SysTick->VAL; + if (ticks_old == ticks_new) { + return ticks_new * 1000 + 1000 - tick_value_old * 1000 / (SysTick->LOAD + 1); + } else { + return ticks_new * 1000 + 1000 - tick_value_new * 1000 / (SysTick->LOAD + 1); + } +} + +uint64_t BSP_TIME_Get() __attribute__((alias("BSP_TIME_Get_us"))); + +int8_t BSP_TIME_Delay_ms(uint32_t ms) { + uint32_t tick_period = 1000u / osKernelGetTickFreq(); + uint32_t ticks = ms / tick_period; + + switch (osKernelGetState()) { + case osKernelError: + case osKernelReserved: + case osKernelLocked: + case osKernelSuspended: + return BSP_ERR; + + case osKernelRunning: + osDelay(ticks ? ticks : 1); + break; + + case osKernelInactive: + case osKernelReady: + HAL_Delay(ms); + break; + } + return BSP_OK; +} + +/*阻塞us延迟*/ +int8_t BSP_TIME_Delay_us(uint32_t us) { + uint64_t start = BSP_TIME_Get_us(); + while (BSP_TIME_Get_us() - start < us) { + // 等待us时间 + } + return BSP_OK; +} + +int8_t BSP_TIME_Delay(uint32_t ms) __attribute__((alias("BSP_TIME_Delay_ms"))); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/User/bsp/time.h b/User/bsp/time.h new file mode 100644 index 0000000..c69085b --- /dev/null +++ b/User/bsp/time.h @@ -0,0 +1,43 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ +/* Exported functions prototypes -------------------------------------------- */ +uint32_t BSP_TIME_Get_ms(); + +uint64_t BSP_TIME_Get_us(); + +uint64_t BSP_TIME_Get(); + +int8_t BSP_TIME_Delay_ms(uint32_t ms); + +/*微秒阻塞延时，一般别用*/ +int8_t BSP_TIME_Delay_us(uint32_t us); + +int8_t BSP_TIME_Delay(uint32_t ms); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/bsp/uart.c b/User/bsp/uart.c new file mode 100644 index 0000000..7157e65 --- /dev/null +++ b/User/bsp/uart.c @@ -0,0 +1,159 @@ +/* Includes ----------------------------------------------------------------- */ +#include + +#include "bsp/uart.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static void (*UART_Callback[BSP_UART_NUM][BSP_UART_CB_NUM])(void); + +/* Private function -------------------------------------------------------- */ +static BSP_UART_t UART_Get(UART_HandleTypeDef *huart) { + if (huart->Instance == UART5) + return BSP_UART_DR16; + else if (huart->Instance == USART2) + return BSP_UART_IMU; + else + return BSP_UART_ERR; +} + +void HAL_UART_TxCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_TX_CPLT_CB](); + } + } +} + +void HAL_UART_TxHalfCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_TX_HALF_CPLT_CB](); + } + } +} + +void HAL_UART_RxCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_RX_CPLT_CB](); + } + } +} + +void HAL_UART_RxHalfCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_RX_HALF_CPLT_CB](); + } + } +} + +void HAL_UART_ErrorCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ERROR_CB]) { + UART_Callback[bsp_uart][BSP_UART_ERROR_CB](); + } + } +} + +void HAL_UART_AbortCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_ABORT_CPLT_CB](); + } + } +} + +void HAL_UART_AbortTransmitCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_ABORT_TX_CPLT_CB](); + } + } +} + +void HAL_UART_AbortReceiveCpltCallback(UART_HandleTypeDef *huart) { + BSP_UART_t bsp_uart = UART_Get(huart); + if (bsp_uart != BSP_UART_ERR) { + if (UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB]) { + UART_Callback[bsp_uart][BSP_UART_ABORT_RX_CPLT_CB](); + } + } +} + +/* Exported functions ------------------------------------------------------- */ +void BSP_UART_IRQHandler(UART_HandleTypeDef *huart) { + if (__HAL_UART_GET_FLAG(huart, UART_FLAG_IDLE)) { + __HAL_UART_CLEAR_IDLEFLAG(huart); + if (UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB]) { + UART_Callback[UART_Get(huart)][BSP_UART_IDLE_LINE_CB](); + } + } +} + +UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart) { + switch (uart) { + case BSP_UART_DR16: + return &huart5; + case BSP_UART_IMU: + return &huart2; + default: + return NULL; + } +} + +int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type, + void (*callback)(void)) { + if (callback == NULL) return BSP_ERR_NULL; + if (uart >= BSP_UART_NUM || type >= BSP_UART_CB_NUM) return BSP_ERR; + UART_Callback[uart][type] = callback; + return BSP_OK; +} + +int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) { + if (uart >= BSP_UART_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + + if (dma) { + return HAL_UART_Transmit_DMA(BSP_UART_GetHandle(uart), data, size); + } else { + return HAL_UART_Transmit_IT(BSP_UART_GetHandle(uart), data, size); + } +} + +int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma) { + if (uart >= BSP_UART_NUM) return BSP_ERR; + if (data == NULL || size == 0) return BSP_ERR_NULL; + + if (dma) { + return HAL_UART_Receive_DMA(BSP_UART_GetHandle(uart), data, size); + } else { + return HAL_UART_Receive_IT(BSP_UART_GetHandle(uart), data, size); + } +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/User/bsp/uart.h b/User/bsp/uart.h new file mode 100644 index 0000000..5060aad --- /dev/null +++ b/User/bsp/uart.h @@ -0,0 +1,69 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include +#include + +#include "bsp/bsp.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported types ----------------------------------------------------------- */ + +/* 要添加使用UART的新设备，需要先在此添加对应的枚举值 */ + +/* UART实体枚举，与设备对应 */ +typedef enum { + BSP_UART_DR16, + BSP_UART_IMU, + BSP_UART_NUM, + BSP_UART_ERR, +} BSP_UART_t; + +/* UART支持的中断回调函数类型，具体参考HAL中定义 */ +typedef enum { + BSP_UART_TX_HALF_CPLT_CB, + BSP_UART_TX_CPLT_CB, + BSP_UART_RX_HALF_CPLT_CB, + BSP_UART_RX_CPLT_CB, + BSP_UART_ERROR_CB, + BSP_UART_ABORT_CPLT_CB, + BSP_UART_ABORT_TX_CPLT_CB, + BSP_UART_ABORT_RX_CPLT_CB, + + BSP_UART_IDLE_LINE_CB, + BSP_UART_CB_NUM, +} BSP_UART_Callback_t; + +/* Exported functions prototypes -------------------------------------------- */ + +UART_HandleTypeDef *BSP_UART_GetHandle(BSP_UART_t uart); + +void BSP_UART_IRQHandler(UART_HandleTypeDef *huart); + +int8_t BSP_UART_RegisterCallback(BSP_UART_t uart, BSP_UART_Callback_t type, + void (*callback)(void)); + +int8_t BSP_UART_Transmit(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma); +int8_t BSP_UART_Receive(BSP_UART_t uart, uint8_t *data, uint16_t size, bool dma); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/ahrs.c b/User/component/ahrs.c new file mode 100644 index 0000000..ffdb870 --- /dev/null +++ b/User/component/ahrs.c @@ -0,0 +1,417 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#include "ahrs.h" + +#include + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +#define BETA_IMU (0.033f) +#define BETA_AHRS (0.041f) + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 2 * proportional gain (Kp) */ +static float beta = BETA_IMU; + +/** + * @brief 不使用磁力计计算姿态 + * + * @param ahrs 姿态解算主结构体 + * @param accl 加速度计数据 + * @param gyro 陀螺仪数据 + * @return int8_t 0对应没有错误 + */ +static int8_t AHRS_UpdateIMU(AHRS_t *ahrs, const AHRS_Accl_t *accl, + const AHRS_Gyro_t *gyro) { + if (ahrs == NULL) return -1; + if (accl == NULL) return -1; + if (gyro == NULL) return -1; + + beta = BETA_IMU; + + float ax = accl->x; + float ay = accl->y; + float az = accl->z; + + float gx = gyro->x; + float gy = gyro->y; + float gz = gyro->z; + + float recip_norm; + float s0, s1, s2, s3; + float q_dot1, q_dot2, q_dot3, q_dot4; + float _2q0, _2q1, _2q2, _2q3, _4q0, _4q1, _4q2, _8q1, _8q2, q0q0, q1q1, q2q2, + q3q3; + + /* Rate of change of quaternion from gyroscope */ + q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - + ahrs->quat.q3 * gz); + q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - + ahrs->quat.q3 * gy); + q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + + ahrs->quat.q3 * gx); + q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - + ahrs->quat.q2 * gx); + + /* Compute feedback only if accelerometer measurement valid (avoids NaN in + * accelerometer normalisation) */ + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + /* Normalise accelerometer measurement */ + recip_norm = InvSqrt(ax * ax + ay * ay + az * az); + ax *= recip_norm; + ay *= recip_norm; + az *= recip_norm; + + /* Auxiliary variables to avoid repeated arithmetic */ + _2q0 = 2.0f * ahrs->quat.q0; + _2q1 = 2.0f * ahrs->quat.q1; + _2q2 = 2.0f * ahrs->quat.q2; + _2q3 = 2.0f * ahrs->quat.q3; + _4q0 = 4.0f * ahrs->quat.q0; + _4q1 = 4.0f * ahrs->quat.q1; + _4q2 = 4.0f * ahrs->quat.q2; + _8q1 = 8.0f * ahrs->quat.q1; + _8q2 = 8.0f * ahrs->quat.q2; + q0q0 = ahrs->quat.q0 * ahrs->quat.q0; + q1q1 = ahrs->quat.q1 * ahrs->quat.q1; + q2q2 = ahrs->quat.q2 * ahrs->quat.q2; + q3q3 = ahrs->quat.q3 * ahrs->quat.q3; + + /* Gradient decent algorithm corrective step */ + s0 = _4q0 * q2q2 + _2q2 * ax + _4q0 * q1q1 - _2q1 * ay; + s1 = _4q1 * q3q3 - _2q3 * ax + 4.0f * q0q0 * ahrs->quat.q1 - + _2q0 * ay - _4q1 + _8q1 * q1q1 + _8q1 * q2q2 + _4q1 * az; + s2 = 4.0f * q0q0 * ahrs->quat.q2 + _2q0 * ax + _4q2 * q3q3 - + _2q3 * ay - _4q2 + _8q2 * q1q1 + _8q2 * q2q2 + _4q2 * az; + s3 = 4.0f * q1q1 * ahrs->quat.q3 - _2q1 * ax + + 4.0f * q2q2 * ahrs->quat.q3 - _2q2 * ay; + + /* normalise step magnitude */ + recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + + s0 *= recip_norm; + s1 *= recip_norm; + s2 *= recip_norm; + s3 *= recip_norm; + + /* Apply feedback step */ + q_dot1 -= beta * s0; + q_dot2 -= beta * s1; + q_dot3 -= beta * s2; + q_dot4 -= beta * s3; + } + + /* Integrate rate of change of quaternion to yield quaternion */ + ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq; + ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq; + ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq; + ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq; + + /* Normalise quaternion */ + recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 + + ahrs->quat.q1 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q2 + + ahrs->quat.q3 * ahrs->quat.q3); + ahrs->quat.q0 *= recip_norm; + ahrs->quat.q1 *= recip_norm; + ahrs->quat.q2 *= recip_norm; + ahrs->quat.q3 *= recip_norm; + + return 0; +} + +/** + * @brief 初始化姿态解算 + * + * @param ahrs 姿态解算主结构体 + * @param magn 磁力计数据 + * @param sample_freq 采样频率 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq) { + if (ahrs == NULL) return -1; + + ahrs->inv_sample_freq = 1.0f / sample_freq; + + ahrs->quat.q0 = 1.0f; + ahrs->quat.q1 = 0.0f; + ahrs->quat.q2 = 0.0f; + ahrs->quat.q3 = 0.0f; + + if (magn) { + float yaw = -atan2(magn->y, magn->x); + + if ((magn->x == 0.0f) && (magn->y == 0.0f) && (magn->z == 0.0f)) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + + } else if ((yaw < (M_PI / 2.0f)) || (yaw > 0.0f)) { + ahrs->quat.q0 = 0.997458339f; + ahrs->quat.q1 = 0.000336312107f; + ahrs->quat.q2 = -0.0057230792f; + ahrs->quat.q3 = 0.0740156546; + + } else if ((yaw < M_PI) || (yaw > (M_PI / 2.0f))) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + + } else if ((yaw < 90.0f) || (yaw > M_PI)) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + + } else if ((yaw < 90.0f) || (yaw > 0.0f)) { + ahrs->quat.q0 = 0.800884545f; + ahrs->quat.q1 = 0.00862364192f; + ahrs->quat.q2 = -0.00283267116f; + ahrs->quat.q3 = 0.598749936f; + } + } + return 0; +} + +/** + * @brief 姿态运算更新一次 + * @note 输入数据必须是NED(North East Down) 参考坐标系 + * + * @param ahrs 姿态解算主结构体 + * @param accl 加速度计数据 + * @param gyro 陀螺仪数据 + * @param magn 磁力计数据 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl, + const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn) { + if (ahrs == NULL) return -1; + if (accl == NULL) return -1; + if (gyro == NULL) return -1; + + beta = BETA_AHRS; + + float recip_norm; + float s0, s1, s2, s3; + float q_dot1, q_dot2, q_dot3, q_dot4; + float hx, hy; + float _2q0mx, _2q0my, _2q0mz, _2q1mx, _2bx, _2bz, _4bx, _4bz, _2q0, _2q1, + _2q2, _2q3, _2q0q2, _2q2q3, q0q0, q0q1, q0q2, q0q3, q1q1, q1q2, q1q3, + q2q2, q2q3, q3q3; + + if (magn == NULL) return AHRS_UpdateIMU(ahrs, accl, gyro); + + float mx = magn->x; + float my = magn->y; + float mz = magn->z; + + /* Use IMU algorithm if magnetometer measurement invalid (avoids NaN in */ + /* magnetometer normalisation) */ + if ((mx == 0.0f) && (my == 0.0f) && (mz == 0.0f)) { + return AHRS_UpdateIMU(ahrs, accl, gyro); + } + + float ax = accl->x; + float ay = accl->y; + float az = accl->z; + + float gx = gyro->x; + float gy = gyro->y; + float gz = gyro->z; + + /* Rate of change of quaternion from gyroscope */ + q_dot1 = 0.5f * (-ahrs->quat.q1 * gx - ahrs->quat.q2 * gy - + ahrs->quat.q3 * gz); + q_dot2 = 0.5f * (ahrs->quat.q0 * gx + ahrs->quat.q2 * gz - + ahrs->quat.q3 * gy); + q_dot3 = 0.5f * (ahrs->quat.q0 * gy - ahrs->quat.q1 * gz + + ahrs->quat.q3 * gx); + q_dot4 = 0.5f * (ahrs->quat.q0 * gz + ahrs->quat.q1 * gy - + ahrs->quat.q2 * gx); + + /* Compute feedback only if accelerometer measurement valid (avoids NaN in + * accelerometer normalisation) */ + if (!((ax == 0.0f) && (ay == 0.0f) && (az == 0.0f))) { + /* Normalise accelerometer measurement */ + recip_norm = InvSqrt(ax * ax + ay * ay + az * az); + ax *= recip_norm; + ay *= recip_norm; + az *= recip_norm; + + /* Normalise magnetometer measurement */ + recip_norm = InvSqrt(mx * mx + my * my + mz * mz); + mx *= recip_norm; + my *= recip_norm; + mz *= recip_norm; + + /* Auxiliary variables to avoid repeated arithmetic */ + _2q0mx = 2.0f * ahrs->quat.q0 * mx; + _2q0my = 2.0f * ahrs->quat.q0 * my; + _2q0mz = 2.0f * ahrs->quat.q0 * mz; + _2q1mx = 2.0f * ahrs->quat.q1 * mx; + _2q0 = 2.0f * ahrs->quat.q0; + _2q1 = 2.0f * ahrs->quat.q1; + _2q2 = 2.0f * ahrs->quat.q2; + _2q3 = 2.0f * ahrs->quat.q3; + _2q0q2 = 2.0f * ahrs->quat.q0 * ahrs->quat.q2; + _2q2q3 = 2.0f * ahrs->quat.q2 * ahrs->quat.q3; + q0q0 = ahrs->quat.q0 * ahrs->quat.q0; + q0q1 = ahrs->quat.q0 * ahrs->quat.q1; + q0q2 = ahrs->quat.q0 * ahrs->quat.q2; + q0q3 = ahrs->quat.q0 * ahrs->quat.q3; + q1q1 = ahrs->quat.q1 * ahrs->quat.q1; + q1q2 = ahrs->quat.q1 * ahrs->quat.q2; + q1q3 = ahrs->quat.q1 * ahrs->quat.q3; + q2q2 = ahrs->quat.q2 * ahrs->quat.q2; + q2q3 = ahrs->quat.q2 * ahrs->quat.q3; + q3q3 = ahrs->quat.q3 * ahrs->quat.q3; + + /* Reference direction of Earth's magnetic field */ + hx = mx * q0q0 - _2q0my * ahrs->quat.q3 + + _2q0mz * ahrs->quat.q2 + mx * q1q1 + + _2q1 * my * ahrs->quat.q2 + _2q1 * mz * ahrs->quat.q3 - + mx * q2q2 - mx * q3q3; + hy = _2q0mx * ahrs->quat.q3 + my * q0q0 - + _2q0mz * ahrs->quat.q1 + _2q1mx * ahrs->quat.q2 - + my * q1q1 + my * q2q2 + _2q2 * mz * ahrs->quat.q3 - my * q3q3; + // _2bx = sqrtf(hx * hx + hy * hy); + // 改为invsqrt + _2bx = 1.f / InvSqrt(hx * hx + hy * hy); + _2bz = -_2q0mx * ahrs->quat.q2 + _2q0my * ahrs->quat.q1 + + mz * q0q0 + _2q1mx * ahrs->quat.q3 - mz * q1q1 + + _2q2 * my * ahrs->quat.q3 - mz * q2q2 + mz * q3q3; + _4bx = 2.0f * _2bx; + _4bz = 2.0f * _2bz; + + /* Gradient decent algorithm corrective step */ + s0 = -_2q2 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q1 * (2.0f * q0q1 + _2q2q3 - ay) - + _2bz * ahrs->quat.q2 * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (-_2bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + _2bx * ahrs->quat.q2 * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s1 = _2q3 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q0 * (2.0f * q0q1 + _2q2q3 - ay) - + 4.0f * ahrs->quat.q1 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + + _2bz * ahrs->quat.q3 * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (_2bx * ahrs->quat.q2 + _2bz * ahrs->quat.q0) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + (_2bx * ahrs->quat.q3 - _4bz * ahrs->quat.q1) * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s2 = -_2q0 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q3 * (2.0f * q0q1 + _2q2q3 - ay) - + 4.0f * ahrs->quat.q2 * (1 - 2.0f * q1q1 - 2.0f * q2q2 - az) + + (-_4bx * ahrs->quat.q2 - _2bz * ahrs->quat.q0) * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (_2bx * ahrs->quat.q1 + _2bz * ahrs->quat.q3) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + (_2bx * ahrs->quat.q0 - _4bz * ahrs->quat.q2) * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + s3 = _2q1 * (2.0f * q1q3 - _2q0q2 - ax) + + _2q2 * (2.0f * q0q1 + _2q2q3 - ay) + + (-_4bx * ahrs->quat.q3 + _2bz * ahrs->quat.q1) * + (_2bx * (0.5f - q2q2 - q3q3) + _2bz * (q1q3 - q0q2) - mx) + + (-_2bx * ahrs->quat.q0 + _2bz * ahrs->quat.q2) * + (_2bx * (q1q2 - q0q3) + _2bz * (q0q1 + q2q3) - my) + + _2bx * ahrs->quat.q1 * + (_2bx * (q0q2 + q1q3) + _2bz * (0.5f - q1q1 - q2q2) - mz); + /* normalise step magnitude */ + recip_norm = InvSqrt(s0 * s0 + s1 * s1 + s2 * s2 + s3 * s3); + s0 *= recip_norm; + s1 *= recip_norm; + s2 *= recip_norm; + s3 *= recip_norm; + + /* Apply feedback step */ + q_dot1 -= beta * s0; + q_dot2 -= beta * s1; + q_dot3 -= beta * s2; + q_dot4 -= beta * s3; + } + + /* Integrate rate of change of quaternion to yield quaternion */ + ahrs->quat.q0 += q_dot1 * ahrs->inv_sample_freq; + ahrs->quat.q1 += q_dot2 * ahrs->inv_sample_freq; + ahrs->quat.q2 += q_dot3 * ahrs->inv_sample_freq; + ahrs->quat.q3 += q_dot4 * ahrs->inv_sample_freq; + + /* Normalise quaternion */ + recip_norm = InvSqrt(ahrs->quat.q0 * ahrs->quat.q0 + + ahrs->quat.q1 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q2 + + ahrs->quat.q3 * ahrs->quat.q3); + ahrs->quat.q0 *= recip_norm; + ahrs->quat.q1 *= recip_norm; + ahrs->quat.q2 *= recip_norm; + ahrs->quat.q3 *= recip_norm; + + return 0; +} + +/** + * @brief 通过姿态解算主结构体中的四元数计算欧拉角 + * + * @param eulr 欧拉角 + * @param ahrs 姿态解算主结构体 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs) { + if (eulr == NULL) return -1; + if (ahrs == NULL) return -1; + + const float sinr_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q3); + const float cosr_cosp = + 1.0f - 2.0f * (ahrs->quat.q1 * ahrs->quat.q1 + + ahrs->quat.q2 * ahrs->quat.q2); + eulr->pit = atan2f(sinr_cosp, cosr_cosp); + + const float sinp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q2 - + ahrs->quat.q3 * ahrs->quat.q1); + + if (fabsf(sinp) >= 1.0f) + eulr->rol = copysignf(M_PI / 2.0f, sinp); + else + eulr->rol = asinf(sinp); + + const float siny_cosp = 2.0f * (ahrs->quat.q0 * ahrs->quat.q3 + + ahrs->quat.q1 * ahrs->quat.q2); + const float cosy_cosp = + 1.0f - 2.0f * (ahrs->quat.q2 * ahrs->quat.q2 + + ahrs->quat.q3 * ahrs->quat.q3); + eulr->yaw = atan2f(siny_cosp, cosy_cosp); + +#if 0 + eulr->yaw *= M_RAD2DEG_MULT; + eulr->rol *= M_RAD2DEG_MULT; + eulr->pit *= M_RAD2DEG_MULT; +#endif + + return 0; +} + +/** + * \brief 将对应数据置零 + * + * \param eulr 被操作的数据 + */ +void AHRS_ResetEulr(AHRS_Eulr_t *eulr) { memset(eulr, 0, sizeof(*eulr)); } + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/component/ahrs.h b/User/component/ahrs.h new file mode 100644 index 0000000..2245b1f --- /dev/null +++ b/User/component/ahrs.h @@ -0,0 +1,114 @@ +/* + 开源的AHRS算法。 + MadgwickAHRS +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include "user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* 欧拉角（Euler angle） */ +typedef struct { + float yaw; /* 偏航角（Yaw angle） */ + float pit; /* 俯仰角（Pitch angle） */ + float rol; /* 翻滚角（Roll angle） */ +} AHRS_Eulr_t; + +/* 加速度计 Accelerometer */ +typedef struct { + float x; + float y; + float z; +} AHRS_Accl_t; + +/* 陀螺仪 Gyroscope */ +typedef struct { + float x; + float y; + float z; +} AHRS_Gyro_t; + +/* 磁力计 Magnetometer */ +typedef struct { + float x; + float y; + float z; +} AHRS_Magn_t; + +/* 四元数 */ +typedef struct { + float q0; + float q1; + float q2; + float q3; +} AHRS_Quaternion_t; + +/* 姿态解算算法主结构体 */ +typedef struct { + /* 四元数 */ + AHRS_Quaternion_t quat; + + float inv_sample_freq; /* 采样频率的的倒数 */ +} AHRS_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/** + * @brief 初始化姿态解算 + * + * @param ahrs 姿态解算主结构体 + * @param magn 磁力计数据 + * @param sample_freq 采样频率 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Init(AHRS_t *ahrs, const AHRS_Magn_t *magn, float sample_freq); + +/** + * @brief 姿态运算更新一次 + * + * @param ahrs 姿态解算主结构体 + * @param accl 加速度计数据 + * @param gyro 陀螺仪数据 + * @param magn 磁力计数据 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_Update(AHRS_t *ahrs, const AHRS_Accl_t *accl, + const AHRS_Gyro_t *gyro, const AHRS_Magn_t *magn); + +/** + * @brief 通过姿态解算主结构体中的四元数计算欧拉角 + * + * @param eulr 欧拉角 + * @param ahrs 姿态解算主结构体 + * @return int8_t 0对应没有错误 + */ +int8_t AHRS_GetEulr(AHRS_Eulr_t *eulr, const AHRS_t *ahrs); + +/** + * \brief 将对应数据置零 + * + * \param eulr 被操作的数据 + */ +void AHRS_ResetEulr(AHRS_Eulr_t *eulr); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/component/component_config.yaml b/User/component/component_config.yaml new file mode 100644 index 0000000..4caf761 --- /dev/null +++ b/User/component/component_config.yaml @@ -0,0 +1,7 @@ +ahrs: + dependencies: + - component/user_math.h + enabled: true +user_math: + dependencies: [] + enabled: true diff --git a/User/component/user_math.c b/User/component/user_math.c new file mode 100644 index 0000000..5e0b0c4 --- /dev/null +++ b/User/component/user_math.c @@ -0,0 +1,134 @@ +/* + 自定义的数学运算。 +*/ + +#include "user_math.h" +#include +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +inline float InvSqrt(float x) { +//#if 0 + /* Fast inverse square-root */ + /* See: http://en.wikipedia.org/wiki/Fast_inverse_square_root */ + float halfx = 0.5f * x; + float y = x; + long i = *(long*)&y; + i = 0x5f3759df - (i>>1); + y = *(float*)&i; + y = y * (1.5f - (halfx * y * y)); + y = y * (1.5f - (halfx * y * y)); + return y; +//#else +// return 1.0f / sqrtf(x); +//#endif +} + +inline float AbsClip(float in, float limit) { + return (in < -limit) ? -limit : ((in > limit) ? limit : in); +} + +float fAbs(float in){ + return (in > 0) ? in : -in; +} + +inline void Clip(float *origin, float min, float max) { + if (*origin > max) *origin = max; + if (*origin < min) *origin = min; +} + +inline float Sign(float in) { return (in > 0) ? 1.0f : 0.0f; } + +/** + * \brief 将运动向量置零 + * + * \param mv 被操作的值 + */ +inline void ResetMoveVector(MoveVector_t *mv) { memset(mv, 0, sizeof(*mv)); } + +/** + * \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x + * 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b; + * \param sp 设定值 + * \param fb 反馈值 + * \param range 被操作的值变化范围，正数时起效 + * \return 函数运行结果 + */ +inline float CircleError(float sp, float fb, float range) { + float error = sp - fb; + if (range > 0.0f) { + float half_range = range / 2.0f; + + if (error > half_range) + error -= range; + else if (error < -half_range) + error += range; + } + return error; +} + +/** + * \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况 + * \param origin 被操作的值 + * \param delta 变化量 + * \param range 被操作的值变化范围，正数时起效 + */ +inline void CircleAdd(float *origin, float delta, float range) { + float out = *origin + delta; + if (range > 0.0f) { + if (out >= range) + out -= range; + else if (out < 0.0f) + out += range; + } + *origin = out; +} + +/** + * @brief 循环值取反 + * + * @param origin 被操作的值 + */ +inline void CircleReverse(float *origin) { *origin = -(*origin) + M_2PI; } + +/** + * @brief 根据目标弹丸速度计算摩擦轮转速 + * + * @param bullet_speed 弹丸速度 + * @param fric_radius 摩擦轮半径 + * @param is17mm 是否为17mm + * @return 摩擦轮转速 + */ +inline float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm) { + if (bullet_speed == 0.0f) return 0.f; + if (is17mm) { + if (bullet_speed == 15.0f) return 4670.f; + if (bullet_speed == 18.0f) return 5200.f; + if (bullet_speed == 30.0f) return 7350.f; + } else { + if (bullet_speed == 10.0f) return 4450.f; + if (bullet_speed == 16.0f) return 5800.f; + } + + /* 不为裁判系统设定值时,计算转速 */ + return 60.0f * (float)bullet_speed / (M_2PI * fric_radius); +} + +// /** +// * @brief 断言失败处理 +// * +// * @param file 文件名 +// * @param line 行号 +// */ +// void VerifyFailed(const char *file, uint32_t line) { +// UNUSED(file); +// UNUSED(line); +// while (1) { +// __NOP(); +// } +// } + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/User/component/user_math.h b/User/component/user_math.h new file mode 100644 index 0000000..6e61ca2 --- /dev/null +++ b/User/component/user_math.h @@ -0,0 +1,179 @@ +/* + 自定义的数学运算。 +*/ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include +#include +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +#define M_DEG2RAD_MULT (0.01745329251f) +#define M_RAD2DEG_MULT (57.2957795131f) + +#ifndef M_PI_2 +#define M_PI_2 1.57079632679f +#endif + +#ifndef M_PI +#define M_PI 3.14159265358979323846f +#endif + +#ifndef M_2PI +#define M_2PI 6.28318530717958647692f +#endif + +#ifndef __packed + #define __packed __attribute__((__packed__)) +#endif /* __packed */ + +#define max(a, b) \ + ({ \ + __typeof__(a) _a = (a); \ + __typeof__(b) _b = (b); \ + _a > _b ? _a : _b; \ + }) + +#define min(a, b) \ + ({ \ + __typeof__(a) _a = (a); \ + __typeof__(b) _b = (b); \ + _a < _b ? _a : _b; \ + }) + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + + + +/* 移动向量 */ +typedef struct { + float vx; /* 前后平移 */ + float vy; /* 左右平移 */ + float wz; /* 转动 */ +} MoveVector_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +float InvSqrt(float x); + +float AbsClip(float in, float limit); + +float fAbs(float in); + +void Clip(float *origin, float min, float max); + +float Sign(float in); + +/** + * \brief 将运动向量置零 + * + * \param mv 被操作的值 + */ +void ResetMoveVector(MoveVector_t *mv); + +/** + * \brief 计算循环值的误差，适用于设定值与反馈值均在（x,y）范围内循环的情况，range应设定为y-x + * 例如：（-M_PI,M_PI）range=M_2PI;(0,M_2PI)range=M_2PI;（a,a+b）range=b; + * \param sp 设定值 + * \param fb 反馈值 + * \param range 被操作的值变化范围，正数时起效 + * \return 函数运行结果 + */ +float CircleError(float sp, float fb, float range); + +/** + * \brief 循环加法，适用于被操作的值在（0,range）范围内循环的情况 + * \param origin 被操作的值 + * \param delta 变化量 + * \param range 被操作的值变化范围，正数时起效 + */ +void CircleAdd(float *origin, float delta, float range); + +/** + * @brief 循环值取反 + * + * @param origin 被操作的值 + */ +void CircleReverse(float *origin); + +/** + * @brief 根据目标弹丸速度计算摩擦轮转速 + * + * @param bullet_speed 弹丸速度 + * @param fric_radius 摩擦轮半径 + * @param is17mm 是否为17mm + * @return 摩擦轮转速 + */ +float CalculateRpm(float bullet_speed, float fric_radius, bool is17mm); + +#ifdef __cplusplus +} +#endif + +#ifdef DEBUG + +/** + * @brief 如果表达式的值为假则运行处理函数 + * + */ +#define ASSERT(expr) \ + do { \ + if (!(expr)) { \ + VerifyFailed(__FILE__, __LINE__); \ + } \ + } while (0) +#else + +/** + * @brief 未定DEBUG，表达式不会运行，断言被忽略 + * + */ +#define ASSERT(expr) ((void)(0)) +#endif + +#ifdef DEBUG + +/** + * @brief 如果表达式的值为假则运行处理函数 + * + */ +#define VERIFY(expr) \ + do { \ + if (!(expr)) { \ + VerifyFailed(__FILE__, __LINE__); \ + } \ + } while (0) +#else + +/** + * @brief 表达式会运行，忽略表达式结果 + * + */ +#define VERIFY(expr) ((void)(expr)) +#endif + +// /** +// * @brief 断言失败处理 +// * +// * @param file 文件名 +// * @param line 行号 +// */ +// void VerifyFailed(const char *file, uint32_t line); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ \ No newline at end of file diff --git a/User/device/bmi088.c b/User/device/bmi088.c new file mode 100644 index 0000000..0678f3c --- /dev/null +++ b/User/device/bmi088.c @@ -0,0 +1,381 @@ +/* + BMI088 陀螺仪+加速度计传感器。 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "bmi088.h" + +#include +#include +#include +#include + +#include "bsp/time.h" +#include "bsp/gpio.h" +#include "bsp/spi.h" +#include "component/user_math.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private define ----------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +#define BMI088_REG_ACCL_CHIP_ID (0x00) +#define BMI088_REG_ACCL_ERR (0x02) +#define BMI088_REG_ACCL_STATUS (0x03) +#define BMI088_REG_ACCL_X_LSB (0x12) +#define BMI088_REG_ACCL_X_MSB (0x13) +#define BMI088_REG_ACCL_Y_LSB (0x14) +#define BMI088_REG_ACCL_Y_MSB (0x15) +#define BMI088_REG_ACCL_Z_LSB (0x16) +#define BMI088_REG_ACCL_Z_MSB (0x17) +#define BMI088_REG_ACCL_SENSORTIME_0 (0x18) +#define BMI088_REG_ACCL_SENSORTIME_1 (0x19) +#define BMI088_REG_ACCL_SENSORTIME_2 (0x1A) +#define BMI088_REG_ACCL_INT_STAT_1 (0x1D) +#define BMI088_REG_ACCL_TEMP_MSB (0x22) +#define BMI088_REG_ACCL_TEMP_LSB (0x23) +#define BMI088_REG_ACCL_CONF (0x40) +#define BMI088_REG_ACCL_RANGE (0x41) +#define BMI088_REG_ACCL_INT1_IO_CONF (0x53) +#define BMI088_REG_ACCL_INT2_IO_CONF (0x54) +#define BMI088_REG_ACCL_INT1_INT2_MAP_DATA (0x58) +#define BMI088_REG_ACCL_SELF_TEST (0x6D) +#define BMI088_REG_ACCL_PWR_CONF (0x7C) +#define BMI088_REG_ACCL_PWR_CTRL (0x7D) +#define BMI088_REG_ACCL_SOFTRESET (0x7E) + +#define BMI088_REG_GYRO_CHIP_ID (0x00) +#define BMI088_REG_GYRO_X_LSB (0x02) +#define BMI088_REG_GYRO_X_MSB (0x03) +#define BMI088_REG_GYRO_Y_LSB (0x04) +#define BMI088_REG_GYRO_Y_MSB (0x05) +#define BMI088_REG_GYRO_Z_LSB (0x06) +#define BMI088_REG_GYRO_Z_MSB (0x07) +#define BMI088_REG_GYRO_INT_STAT_1 (0x0A) +#define BMI088_REG_GYRO_RANGE (0x0F) +#define BMI088_REG_GYRO_BANDWIDTH (0x10) +#define BMI088_REG_GYRO_LPM1 (0x11) +#define BMI088_REG_GYRO_SOFTRESET (0x14) +#define BMI088_REG_GYRO_INT_CTRL (0x15) +#define BMI088_REG_GYRO_INT3_INT4_IO_CONF (0x16) +#define BMI088_REG_GYRO_INT3_INT4_IO_MAP (0x18) +#define BMI088_REG_GYRO_SELF_TEST (0x3C) + +#define BMI088_CHIP_ID_ACCL (0x1E) +#define BMI088_CHIP_ID_GYRO (0x0F) + +#define BMI088_LEN_RX_BUFF (19) +/* Private macro ------------------------------------------------------------ */ +#define BMI088_ACCL_NSS_SET() \ + BSP_GPIO_WritePin(BSP_GPIO_ACC_CS, GPIO_PIN_SET) +#define BMI088_ACCL_NSS_RESET() \ + BSP_GPIO_WritePin(BSP_GPIO_ACC_CS, GPIO_PIN_RESET) + +#define BMI088_GYRO_NSS_SET() \ + BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_SET) +#define BMI088_GYRO_NSS_RESET() \ + BSP_GPIO_WritePin(BSP_GPIO_GYRO_CS, GPIO_PIN_RESET) + +/* Private typedef ---------------------------------------------------------- */ +typedef enum { + BMI_ACCL, + BMI_GYRO, +} BMI_Device_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ +static uint8_t buffer[2]; +static uint8_t bmi088_rxbuf[BMI088_LEN_RX_BUFF]; + +static osThreadId_t thread_alert; +static bool inited = false; + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +static void BMI_WriteSingle(BMI_Device_t dv, uint8_t reg, uint8_t data) { + buffer[0] = (reg & 0x7f); + buffer[1] = data; + + BSP_TIME_Delay(1); + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 2u, false); + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_SET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_SET(); + break; + } +} + +static uint8_t BMI_ReadSingle(BMI_Device_t dv, uint8_t reg) { + BSP_TIME_Delay(1); + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + buffer[0] = (uint8_t)(reg | 0x80); + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); + BSP_SPI_Receive(BSP_SPI_BMI088, buffer, 2u, false); + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_SET(); + return buffer[1]; + + case BMI_GYRO: + BMI088_GYRO_NSS_SET(); + return buffer[0]; + } +} + +static void BMI_Read(BMI_Device_t dv, uint8_t reg, uint8_t *data, uint8_t len) { + if (data == NULL) return; + + switch (dv) { + case BMI_ACCL: + BMI088_ACCL_NSS_RESET(); + break; + + case BMI_GYRO: + BMI088_GYRO_NSS_RESET(); + break; + } + buffer[0] = (uint8_t)(reg | 0x80); + BSP_SPI_Transmit(BSP_SPI_BMI088, buffer, 1u, false); + BSP_SPI_Receive(BSP_SPI_BMI088, data, len, true); +} + +static void BMI088_RxCpltCallback(void) { + if (BSP_GPIO_ReadPin(BSP_GPIO_ACC_CS) == GPIO_PIN_RESET) { + BMI088_ACCL_NSS_SET(); + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_RAW_REDY); + } + if (BSP_GPIO_ReadPin(BSP_GPIO_GYRO_CS) == GPIO_PIN_RESET) { + BMI088_GYRO_NSS_SET(); + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_RAW_REDY); + } +} + +static void BMI088_AcclIntCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_ACCL_NEW_DATA); +} + +static void BMI088_GyroIntCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_BMI088_GYRO_NEW_DATA); +} + +/* Exported functions ------------------------------------------------------- */ +int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + if (cali == NULL) return DEVICE_ERR_NULL; + if (inited) return DEVICE_ERR_INITED; + if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + + bmi088->cali = cali; + + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_SOFTRESET, 0xB6); + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_SOFTRESET, 0xB6); + BSP_TIME_Delay(30); + + /* Switch accl to SPI mode. */ + BMI_ReadSingle(BMI_ACCL, BMI088_CHIP_ID_ACCL); + + if (BMI_ReadSingle(BMI_ACCL, BMI088_REG_ACCL_CHIP_ID) != BMI088_CHIP_ID_ACCL) + return DEVICE_ERR_NO_DEV; + + if (BMI_ReadSingle(BMI_GYRO, BMI088_REG_GYRO_CHIP_ID) != BMI088_CHIP_ID_GYRO) + return DEVICE_ERR_NO_DEV; + + BSP_GPIO_DisableIRQ(BSP_GPIO_ACC_INT); + BSP_GPIO_DisableIRQ(BSP_GPIO_ACC_INT); + + BSP_SPI_RegisterCallback(BSP_SPI_BMI088, BSP_SPI_RX_CPLT_CB, + BMI088_RxCpltCallback); + BSP_GPIO_RegisterCallback(BSP_GPIO_ACC_INT, BMI088_AcclIntCallback); + BSP_GPIO_RegisterCallback(BSP_GPIO_ACC_INT, BMI088_GyroIntCallback); + + /* Accl init. */ + /* Filter setting: Normal. */ + /* ODR: 0xAB: 800Hz. 0xAA: 400Hz. 0xA9: 200Hz. 0xA8: 100Hz. 0xA6: 25Hz. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_CONF, 0xAA); + + /* 0x00: +-3G. 0x01: +-6G. 0x02: +-12G. 0x03: +-24G. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_RANGE, 0x01); + + /* INT1 as output. Push-pull. Active low. Output. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_IO_CONF, 0x08); + + /* Map data ready interrupt to INT1. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_INT1_INT2_MAP_DATA, 0x04); + + /* Turn on accl. Now we can read data. */ + BMI_WriteSingle(BMI_ACCL, BMI088_REG_ACCL_PWR_CTRL, 0x04); + BSP_TIME_Delay(50); + + /* Gyro init. */ + /* 0x00: +-2000. 0x01: +-1000. 0x02: +-500. 0x03: +-250. 0x04: +-125. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_RANGE, 0x01); + + /* Filter bw: 47Hz. */ + /* ODR: 0x02: 1000Hz. 0x03: 400Hz. 0x06: 200Hz. 0x07: 100Hz. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_BANDWIDTH, 0x03); + + /* INT3 and INT4 as output. Push-pull. Active low. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_CONF, 0x00); + + /* Map data ready interrupt to INT3. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT3_INT4_IO_MAP, 0x01); + + /* Enable new data interrupt. */ + BMI_WriteSingle(BMI_GYRO, BMI088_REG_GYRO_INT_CTRL, 0x80); + + BSP_TIME_Delay(10); + + inited = true; + + BSP_GPIO_EnableIRQ(BSP_GPIO_ACC_INT); + BSP_GPIO_EnableIRQ(BSP_GPIO_ACC_INT); + return DEVICE_OK; +} + +bool BMI088_GyroStable(AHRS_Gyro_t *gyro) { + return ((gyro->x < 0.03f) && (gyro->y < 0.03f) && (gyro->z < 0.03f)); +} + +uint32_t BMI088_WaitNew() { + return osThreadFlagsWait( + SIGNAL_BMI088_ACCL_NEW_DATA | SIGNAL_BMI088_GYRO_NEW_DATA, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_AcclStartDmaRecv() { + BMI_Read(BMI_ACCL, BMI088_REG_ACCL_X_LSB, bmi088_rxbuf, BMI088_LEN_RX_BUFF); + return DEVICE_OK; +} + +uint32_t BMI088_AcclWaitDmaCplt() { + return osThreadFlagsWait(SIGNAL_BMI088_ACCL_RAW_REDY, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_GyroStartDmaRecv() { + BMI_Read(BMI_GYRO, BMI088_REG_GYRO_X_LSB, bmi088_rxbuf + 7, 6u); + return DEVICE_OK; +} + +uint32_t BMI088_GyroWaitDmaCplt() { + return osThreadFlagsWait(SIGNAL_BMI088_GYRO_RAW_REDY, osFlagsWaitAll, + osWaitForever); +} + +int8_t BMI088_ParseAccl(BMI088_t *bmi088) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + +#if 1 + int16_t raw_x, raw_y, raw_z; + memcpy(&raw_x, bmi088_rxbuf + 1, sizeof(raw_x)); + memcpy(&raw_y, bmi088_rxbuf + 3, sizeof(raw_y)); + memcpy(&raw_z, bmi088_rxbuf + 5, sizeof(raw_z)); + + bmi088->accl.x = (float)raw_x; + bmi088->accl.y = (float)raw_y; + bmi088->accl.z = (float)raw_z; + +#else + const int16_t *praw_x = (int16_t *)(bmi088_rxbuf + 1); + const int16_t *praw_y = (int16_t *)(bmi088_rxbuf + 3); + const int16_t *praw_z = (int16_t *)(bmi088_rxbuf + 5); + + bmi088->accl.x = (float)*praw_x; + bmi088->accl.y = (float)*praw_y; + bmi088->accl.z = (float)*praw_z; + +#endif + + /* 3G: 10920. 6G: 5460. 12G: 2730. 24G: 1365. */ + bmi088->accl.x /= 5460.0f; + bmi088->accl.y /= 5460.0f; + bmi088->accl.z /= 5460.0f; + + int16_t raw_temp = + (uint16_t)((bmi088_rxbuf[17] << 3) | (bmi088_rxbuf[18] >> 5)); + + if (raw_temp > 1023) raw_temp -= 2048; + + bmi088->temp = (float)raw_temp * 0.125f + 23.0f; + + return DEVICE_OK; +} + +int8_t BMI088_ParseGyro(BMI088_t *bmi088) { + if (bmi088 == NULL) return DEVICE_ERR_NULL; + +#if 1 + /* Gyroscope imu_raw -> degrees/sec -> radians/sec */ + int16_t raw_x, raw_y, raw_z; + memcpy(&raw_x, bmi088_rxbuf + 7, sizeof(raw_x)); + memcpy(&raw_y, bmi088_rxbuf + 9, sizeof(raw_y)); + memcpy(&raw_z, bmi088_rxbuf + 11, sizeof(raw_z)); + + bmi088->gyro.x = (float)raw_x; + bmi088->gyro.y = (float)raw_y; + bmi088->gyro.z = (float)raw_z; + +#else + /* Gyroscope imu_raw -> degrees/sec -> radians/sec */ + const int16_t *raw_x = (int16_t *)(bmi088_rxbuf + 7); + const int16_t *raw_y = (int16_t *)(bmi088_rxbuf + 9); + const int16_t *raw_z = (int16_t *)(bmi088_rxbuf + 11); + + bmi088->gyro.x = (float)*raw_x; + bmi088->gyro.y = (float)*raw_y; + bmi088->gyro.z = (float)*raw_z; +#endif + + /* FS125: 262.144. FS250: 131.072. FS500: 65.536. FS1000: 32.768. + * FS2000: 16.384.*/ + bmi088->gyro.x /= 32.768f; + bmi088->gyro.y /= 32.768f; + bmi088->gyro.z /= 32.768f; + + bmi088->gyro.x *= M_DEG2RAD_MULT; + bmi088->gyro.y *= M_DEG2RAD_MULT; + bmi088->gyro.z *= M_DEG2RAD_MULT; + + bmi088->gyro.x -= bmi088->cali->gyro_offset.x; + bmi088->gyro.y -= bmi088->cali->gyro_offset.y; + bmi088->gyro.z -= bmi088->cali->gyro_offset.z; + + return DEVICE_ERR_NULL; +} + +float BMI088_GetUpdateFreq(BMI088_t *bmi088) { + (void)bmi088; + return 400.0f; +} diff --git a/User/device/bmi088.h b/User/device/bmi088.h new file mode 100644 index 0000000..eb44e0c --- /dev/null +++ b/User/device/bmi088.h @@ -0,0 +1,81 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include +#include + +#include "component/ahrs.h" +#include "device/device.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + struct { + float x; + float y; + float z; + } gyro_offset; /* 陀螺仪偏置 */ +} BMI088_Cali_t; /* BMI088校准数据 */ + +typedef struct { + DEVICE_Header_t header; + AHRS_Accl_t accl; + AHRS_Gyro_t gyro; + + float temp; /* 温度 */ + + const BMI088_Cali_t *cali; +} BMI088_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ +int8_t BMI088_Init(BMI088_t *bmi088, const BMI088_Cali_t *cali); +int8_t BMI088_Restart(void); + +bool BMI088_GyroStable(AHRS_Gyro_t *gyro); + +/* Sensor use right-handed coordinate system. */ +/* + x < R(logo) + y + UP is z + All implementation should follow this rule. + */ +uint32_t BMI088_WaitNew(); + +/* + BMI088的Accl和Gyro共用同一个DMA通道，所以一次只能读一个传感器。 + 即BMI088_AcclStartDmaRecv() 和 BMI088_AcclWaitDmaCplt() 中间不能 + 出现 BMI088_GyroStartDmaRecv()。 +*/ +int8_t BMI088_AcclStartDmaRecv(); +uint32_t BMI088_AcclWaitDmaCplt(); +int8_t BMI088_GyroStartDmaRecv(); +uint32_t BMI088_GyroWaitDmaCplt(); +int8_t BMI088_ParseAccl(BMI088_t *bmi088); +int8_t BMI088_ParseGyro(BMI088_t *bmi088); +float BMI088_GetUpdateFreq(BMI088_t *bmi088); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/device/device.h b/User/device/device.h new file mode 100644 index 0000000..18ffc73 --- /dev/null +++ b/User/device/device.h @@ -0,0 +1,51 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +#define DEVICE_OK (0) +#define DEVICE_ERR (-1) +#define DEVICE_ERR_NULL (-2) +#define DEVICE_ERR_INITED (-3) +#define DEVICE_ERR_NO_DEV (-4) + +/* AUTO GENERATED SIGNALS BEGIN */ +#define SIGNAL_DR16_RAW_REDY (1u << 0) +#define SIGNAL_BMI088_ACCL_RAW_REDY (1u << 1) +#define SIGNAL_BMI088_GYRO_RAW_REDY (1u << 2) +#define SIGNAL_BMI088_ACCL_NEW_DATA (1u << 3) +#define SIGNAL_BMI088_GYRO_NEW_DATA (1u << 4) +/* AUTO GENERATED SIGNALS END */ + +/* USER SIGNALS BEGIN */ + +/* USER SIGNALS END */ +/*设备层通用Header*/ +typedef struct { + bool online; + uint64_t last_online_time; +} DEVICE_Header_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/device/device_config.yaml b/User/device/device_config.yaml new file mode 100644 index 0000000..834cabd --- /dev/null +++ b/User/device/device_config.yaml @@ -0,0 +1,15 @@ +bmi088: + bsp_config: + BSP_GPIO_ACCL_CS: BSP_GPIO_ACC_CS + BSP_GPIO_ACCL_INT: BSP_GPIO_ACC_INT + BSP_GPIO_GYRO_CS: BSP_GPIO_GYRO_CS + BSP_GPIO_GYRO_INT: BSP_GPIO_ACC_INT + BSP_SPI_BMI088: BSP_SPI_BMI088 + enabled: true +dr16: + bsp_config: + BSP_UART_DR16: BSP_UART_DR16 + enabled: true +motor: + bsp_config: {} + enabled: true diff --git a/User/device/dr16.c b/User/device/dr16.c new file mode 100644 index 0000000..120997b --- /dev/null +++ b/User/device/dr16.c @@ -0,0 +1,169 @@ +/* + DR16接收机 + Example： + + DR16_Init(&dr16); + + while (1) { + DR16_StartDmaRecv(&dr16); + if (DR16_WaitDmaCplt(20)) { + DR16_ParseData(&dr16); + } else { + DR16_Offline(&dr16); + } +} +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "dr16.h" +#include "bsp/uart.h" +#include "bsp/time.h" +#include "device.h" + +#include +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ +/* Private define ----------------------------------------------------------- */ +#define DR16_CH_VALUE_MIN (364u) +#define DR16_CH_VALUE_MID (1024u) +#define DR16_CH_VALUE_MAX (1684u) + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* Private variables -------------------------------------------------------- */ + +static osThreadId_t thread_alert; +static bool inited = false; + +/* Private function -------------------------------------------------------- */ +static void DR16_RxCpltCallback(void) { + osThreadFlagsSet(thread_alert, SIGNAL_DR16_RAW_REDY); +} + +static bool DR16_DataCorrupted(const DR16_t *dr16) { + if (dr16 == NULL) return DEVICE_ERR_NULL; + + if ((dr16->raw_data.ch_r_x < DR16_CH_VALUE_MIN) || + (dr16->raw_data.ch_r_x > DR16_CH_VALUE_MAX)) + return DEVICE_ERR; + + if ((dr16->raw_data.ch_r_y < DR16_CH_VALUE_MIN) || + (dr16->raw_data.ch_r_y > DR16_CH_VALUE_MAX)) + return DEVICE_ERR; + + if ((dr16->raw_data.ch_l_x < DR16_CH_VALUE_MIN) || + (dr16->raw_data.ch_l_x > DR16_CH_VALUE_MAX)) + return DEVICE_ERR; + + if ((dr16->raw_data.ch_l_y < DR16_CH_VALUE_MIN) || + (dr16->raw_data.ch_l_y > DR16_CH_VALUE_MAX)) + return DEVICE_ERR; + + if (dr16->raw_data.sw_l == 0) return DEVICE_ERR; + + if (dr16->raw_data.sw_r == 0) return DEVICE_ERR; + + return DEVICE_OK; +} + +/* Exported functions ------------------------------------------------------- */ +int8_t DR16_Init(DR16_t *dr16) { + if (dr16 == NULL) return DEVICE_ERR_NULL; + if (inited) return DEVICE_ERR_INITED; + if ((thread_alert = osThreadGetId()) == NULL) return DEVICE_ERR_NULL; + + BSP_UART_RegisterCallback(BSP_UART_DR16, BSP_UART_RX_CPLT_CB, + DR16_RxCpltCallback); + + inited = true; + return DEVICE_OK; +} + +int8_t DR16_Restart(void) { + __HAL_UART_DISABLE(BSP_UART_GetHandle(BSP_UART_DR16)); + __HAL_UART_ENABLE(BSP_UART_GetHandle(BSP_UART_DR16)); + return DEVICE_OK; +} + +int8_t DR16_StartDmaRecv(DR16_t *dr16) { + if (HAL_UART_Receive_DMA(BSP_UART_GetHandle(BSP_UART_DR16), + (uint8_t *)&(dr16->raw_data), + sizeof(dr16->raw_data)) == HAL_OK) + return DEVICE_OK; + return DEVICE_ERR; +} + +bool DR16_WaitDmaCplt(uint32_t timeout) { + return (osThreadFlagsWait(SIGNAL_DR16_RAW_REDY, osFlagsWaitAll, timeout) == + SIGNAL_DR16_RAW_REDY); +} + +int8_t DR16_ParseData(DR16_t *dr16){ + if (dr16 == NULL) return DEVICE_ERR_NULL; + + if (DR16_DataCorrupted(dr16)) { + return DEVICE_ERR; + } + + dr16->header.online = true; + dr16->header.last_online_time = BSP_TIME_Get_us(); + + memset(&(dr16->data), 0, sizeof(dr16->data)); + + float full_range = (float)(DR16_CH_VALUE_MAX - DR16_CH_VALUE_MIN); + + // 解析摇杆数据 + dr16->data.ch_r_x = 2.0f * ((float)dr16->raw_data.ch_r_x - DR16_CH_VALUE_MID) / full_range; + dr16->data.ch_r_y = 2.0f * ((float)dr16->raw_data.ch_r_y - DR16_CH_VALUE_MID) / full_range; + dr16->data.ch_l_x = 2.0f * ((float)dr16->raw_data.ch_l_x - DR16_CH_VALUE_MID) / full_range; + dr16->data.ch_l_y = 2.0f * ((float)dr16->raw_data.ch_l_y - DR16_CH_VALUE_MID) / full_range; + + // 解析拨杆位置 + dr16->data.sw_l = (DR16_SwitchPos_t)dr16->raw_data.sw_l; + dr16->data.sw_r = (DR16_SwitchPos_t)dr16->raw_data.sw_r; + + // 解析鼠标数据 + dr16->data.mouse.x = dr16->raw_data.x; + dr16->data.mouse.y = dr16->raw_data.y; + dr16->data.mouse.z = dr16->raw_data.z; + + dr16->data.mouse.l_click = dr16->raw_data.press_l; + dr16->data.mouse.r_click = dr16->raw_data.press_r; + + // 解析键盘按键 - 使用union简化代码 + uint16_t key_value = dr16->raw_data.key; + + // 解析键盘位映射（W-B键，位0-15） + for (int i = DR16_KEY_W; i <= DR16_KEY_B; i++) { + dr16->data.keyboard.key[i] = (key_value & (1 << i)) != 0; + } + + // 解析鼠标点击 + dr16->data.keyboard.key[DR16_L_CLICK] = dr16->data.mouse.l_click; + dr16->data.keyboard.key[DR16_R_CLICK] = dr16->data.mouse.r_click; + + // 解析第五通道 + dr16->data.ch_res = 2.0f * ((float)dr16->raw_data.res - DR16_CH_VALUE_MID) / full_range; + + return DEVICE_OK; +} + +int8_t DR16_Offline(DR16_t *dr16){ + if (dr16 == NULL) return DEVICE_ERR_NULL; + + dr16->header.online = false; + memset(&(dr16->data), 0, sizeof(dr16->data)); + + return DEVICE_OK; +} + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ diff --git a/User/device/dr16.h b/User/device/dr16.h new file mode 100644 index 0000000..03fa526 --- /dev/null +++ b/User/device/dr16.h @@ -0,0 +1,117 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include + +#include "component/user_math.h" +#include "device.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct __packed { + uint16_t ch_r_x : 11; + uint16_t ch_r_y : 11; + uint16_t ch_l_x : 11; + uint16_t ch_l_y : 11; + uint8_t sw_r : 2; + uint8_t sw_l : 2; + int16_t x; + int16_t y; + int16_t z; + uint8_t press_l; + uint8_t press_r; + uint16_t key; + uint16_t res; +} DR16_RawData_t; + +typedef enum { + DR16_SW_ERR = 0, + DR16_SW_UP = 1, + DR16_SW_MID = 3, + DR16_SW_DOWN = 2, +} DR16_SwitchPos_t; + +/* 键盘按键值 */ +typedef enum { + DR16_KEY_W = 0, + DR16_KEY_S, + DR16_KEY_A, + DR16_KEY_D, + DR16_KEY_SHIFT, + DR16_KEY_CTRL, + DR16_KEY_Q, + DR16_KEY_E, + DR16_KEY_R, + DR16_KEY_F, + DR16_KEY_G, + DR16_KEY_Z, + DR16_KEY_X, + DR16_KEY_C, + DR16_KEY_V, + DR16_KEY_B, + DR16_L_CLICK, + DR16_R_CLICK, + DR16_KEY_NUM, +} DR16_Key_t; + +typedef struct { + float ch_l_x; /* 遥控器左侧摇杆横轴值，上为正 */ + float ch_l_y; /* 遥控器左侧摇杆纵轴值，右为正 */ + float ch_r_x; /* 遥控器右侧摇杆横轴值，上为正 */ + float ch_r_y; /* 遥控器右侧摇杆纵轴值，右为正 */ + + float ch_res; /* 第五通道值 */ + + DR16_SwitchPos_t sw_r; /* 右侧拨杆位置 */ + DR16_SwitchPos_t sw_l; /* 左侧拨杆位置 */ + + struct { + int16_t x; + int16_t y; + int16_t z; + bool l_click; /* 左键 */ + bool r_click; /* 右键 */ + } mouse; /* 鼠标值 */ + + union { + bool key[DR16_KEY_NUM]; /* 键盘按键值 */ + uint16_t value; /* 键盘按键值的位映射 */ + } keyboard; + + uint16_t res; /* 保留，未启用 */ +} DR16_Data_t; + +typedef struct { + DEVICE_Header_t header; + DR16_RawData_t raw_data; + DR16_Data_t data; +} DR16_t; + +/* Exported functions prototypes -------------------------------------------- */ +int8_t DR16_Init(DR16_t *dr16); +int8_t DR16_Restart(void); +int8_t DR16_StartDmaRecv(DR16_t *dr16); +bool DR16_WaitDmaCplt(uint32_t timeout); +int8_t DR16_ParseData(DR16_t *dr16); +int8_t DR16_Offline(DR16_t *dr16); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/device/motor.c b/User/device/motor.c new file mode 100644 index 0000000..1fb059d --- /dev/null +++ b/User/device/motor.c @@ -0,0 +1,52 @@ +/* + 电机通用函数 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "motor.h" + +#include + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + + +/* Private define ----------------------------------------------------------- */ +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Private macro ------------------------------------------------------------ */ +/* Private typedef ---------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private variables -------------------------------------------------------- */ + +/* Private function -------------------------------------------------------- */ +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +/* Exported functions ------------------------------------------------------- */ +float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.rotor_abs_angle; +} + +float MOTOR_GetRotorSpeed(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.rotor_speed; +} + +float MOTOR_GetTorqueCurrent(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.torque_current; +} + +float MOTOR_GetTemp(const MOTOR_t *motor) { + if (motor == NULL) return DEVICE_ERR_NULL; + return motor->feedback.temp; +} diff --git a/User/device/motor.h b/User/device/motor.h new file mode 100644 index 0000000..e1f945b --- /dev/null +++ b/User/device/motor.h @@ -0,0 +1,68 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +/* Includes ----------------------------------------------------------------- */ +#include "device/device.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* USER DEFINE BEGIN */ + +/* USER DEFINE END */ + +/* Exported constants ------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ +typedef struct { + float rotor_abs_angle; /* 转子绝对角度 */ + float rotor_speed; /* 实际转子转速 */ + float torque_current; /* 转矩电流 */ + float temp; /* 温度 */ +} MOTOR_Feedback_t; + +/** + * @brief mit电机输出参数结构体 + */ +typedef struct { + float torque; /* 目标力矩 */ + float velocity; /* 目标速度 */ + float angle; /* 目标位置 */ + float kp; /* 位置环增益 */ + float kd; /* 速度环增益 */ +} MOTOR_MIT_Output_t; + +/** + * @brief 转矩电流控制模式参数结构体 + */ +typedef struct { + float current; /* 目标电流 */ +} MOTOR_Current_Output_t; + +typedef struct { + DEVICE_Header_t header; + bool reverse; /* 是否反装 true表示反装 */ + MOTOR_Feedback_t feedback; +} MOTOR_t; + +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Exported functions prototypes -------------------------------------------- */ +float MOTOR_GetRotorAbsAngle(const MOTOR_t *motor); +float MOTOR_GetRotorSpeed(const MOTOR_t *motor); +float MOTOR_GetTorqueCurrent(const MOTOR_t *motor); +float MOTOR_GetTemp(const MOTOR_t *motor); + +/* USER FUNCTION BEGIN */ + +/* USER FUNCTION END */ + +#ifdef __cplusplus +} +#endif diff --git a/User/module/config.c b/User/module/config.c new file mode 100644 index 0000000..99a8640 --- /dev/null +++ b/User/module/config.c @@ -0,0 +1,38 @@ +/* + * 配置相关 + */ + +/* Includes ----------------------------------------------------------------- */ +#include "module/config.h" + + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ + +/* Exported variables ------------------------------------------------------- */ + +/** + * @brief 机器人参数配置 + * @note 在此配置机器人参数 + */ +Config_RobotParam_t robot_config = { + /* USER CODE BEGIN robot_config */ + .example_param = 0, // 示例参数初始化 + + // 在此添加您的配置参数初始化 + + /* USER CODE END robot_config */ +}; + +/* Private function prototypes ---------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +/** + * @brief 获取机器人配置参数 + * @return 机器人配置参数指针 + */ +Config_RobotParam_t* Config_GetRobotParam(void) { + return &robot_config; +} \ No newline at end of file diff --git a/User/module/config.h b/User/module/config.h new file mode 100644 index 0000000..4478d9e --- /dev/null +++ b/User/module/config.h @@ -0,0 +1,38 @@ +/* + * 配置相关 + */ + +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif + +#include +#include + +/** + * @brief 机器人参数配置结构体 + * @note 在此添加您的配置参数 + */ +typedef struct { + // 示例配置项（可根据实际需求修改或删除） + uint8_t example_param; // 示例参数 + + /* USER CODE BEGIN Config_RobotParam */ + // 在此添加您的配置参数 + + /* USER CODE END Config_RobotParam */ +} Config_RobotParam_t; + +/* Exported functions prototypes -------------------------------------------- */ + +/** + * @brief 获取机器人配置参数 + * @return 机器人配置参数指针 + */ +Config_RobotParam_t* Config_GetRobotParam(void); + +#ifdef __cplusplus +} +#endif diff --git a/User/task/ai.c b/User/task/ai.c new file mode 100644 index 0000000..ae90c59 --- /dev/null +++ b/User/task/ai.c @@ -0,0 +1,44 @@ +/* + ai Task + +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* USER STRUCT BEGIN */ + +/* USER STRUCT END */ + +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ +void Task_ai(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + + + /* 计算任务运行到指定频率需要等待的tick数 */ + const uint32_t delay_tick = osKernelGetTickFreq() / AI_FREQ; + + osDelay(AI_INIT_DELAY); /* 延时一段时间再开启任务 */ + + uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + + while (1) { + tick += delay_tick; /* 计算下一个唤醒时刻 */ + /* USER CODE BEGIN */ + + /* USER CODE END */ + osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ + } + +} \ No newline at end of file diff --git a/User/task/config.yaml b/User/task/config.yaml new file mode 100644 index 0000000..a60cb69 --- /dev/null +++ b/User/task/config.yaml @@ -0,0 +1,7 @@ +- delay: 0 + description: '' + freq_control: true + frequency: 500.0 + function: Task_ai + name: ai + stack: 256 diff --git a/User/task/init.c b/User/task/init.c new file mode 100644 index 0000000..5e45f96 --- /dev/null +++ b/User/task/init.c @@ -0,0 +1,42 @@ +/* + Init Task + 任务初始化，创建各个线程任务和消息队列 +*/ + +/* Includes ----------------------------------------------------------------- */ +#include "task/user_task.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ + +/* Private typedef ---------------------------------------------------------- */ +/* Private define ----------------------------------------------------------- */ +/* Private macro ------------------------------------------------------------ */ +/* Private variables -------------------------------------------------------- */ +/* Private function --------------------------------------------------------- */ +/* Exported functions ------------------------------------------------------- */ + +/** + * \brief 初始化 + * + * \param argument 未使用 + */ +void Task_Init(void *argument) { + (void)argument; /* 未使用argument，消除警告 */ + /* USER CODE INIT BEGIN */ + + /* USER CODE INIT END */ + osKernelLock(); /* 锁定内核，防止任务切换 */ + + /* 创建任务线程 */ + task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai); + + // 创建消息队列 + /* USER MESSAGE BEGIN */ + task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL); + /* USER MESSAGE END */ + + osKernelUnlock(); // 解锁内核 + osThreadTerminate(osThreadGetId()); // 任务完成后结束自身 +} \ No newline at end of file diff --git a/User/task/user_task.c b/User/task/user_task.c new file mode 100644 index 0000000..fab1c01 --- /dev/null +++ b/User/task/user_task.c @@ -0,0 +1,16 @@ +#include "task/user_task.h" + +Task_Runtime_t task_runtime; + +const osThreadAttr_t attr_init = { + .name = "Task_Init", + .priority = osPriorityRealtime, + .stack_size = 256 * 4, +}; + +/* User_task */ +const osThreadAttr_t attr_ai = { + .name = "ai", + .priority = osPriorityNormal, + .stack_size = 256 * 4, +}; \ No newline at end of file diff --git a/User/task/user_task.h b/User/task/user_task.h new file mode 100644 index 0000000..8b52a53 --- /dev/null +++ b/User/task/user_task.h @@ -0,0 +1,80 @@ +#pragma once + +#ifdef __cplusplus +extern "C" { +#endif +/* Includes ----------------------------------------------------------------- */ +#include +#include "FreeRTOS.h" +#include "task.h" + +/* USER INCLUDE BEGIN */ + +/* USER INCLUDE END */ +/* Exported constants ------------------------------------------------------- */ +/* 任务运行频率 */ +#define AI_FREQ (500.0) + +/* 任务初始化延时ms */ +#define TASK_INIT_DELAY (100u) +#define AI_INIT_DELAY (0) + +/* Exported defines --------------------------------------------------------- */ +/* Exported macro ----------------------------------------------------------- */ +/* Exported types ----------------------------------------------------------- */ + +/* 任务运行时结构体 */ +typedef struct { + /* 各任务，也可以叫做线程 */ + struct { + osThreadId_t ai; + } thread; + + /* USER MESSAGE BEGIN */ + struct { + osMessageQueueId_t user_msg; /* 用户自定义任务消息队列 */ + } msgq; + /* USER MESSAGE END */ + + /* 机器人状态 */ + struct { + float battery; /* 电池电量百分比 */ + float vbat; /* 电池电压 */ + float cpu_temp; /* CPU温度 */ + } status; + + /* USER CONFIG BEGIN */ + + /* USER CONFIG END */ + + /* 各任务的stack使用 */ + struct { + UBaseType_t ai; + } stack_water_mark; + + /* 各任务运行频率 */ + struct { + float ai; + } freq; + + /* 任务最近运行时间 */ + struct { + float ai; + } last_up_time; + +} Task_Runtime_t; + +/* 任务运行时结构体 */ +extern Task_Runtime_t task_runtime; + +/* 初始化任务句柄 */ +extern const osThreadAttr_t attr_init; +extern const osThreadAttr_t attr_ai; + +/* 任务函数声明 */ +void Task_Init(void *argument); +void Task_ai(void *argument); + +#ifdef __cplusplus +} +#endif \ No newline at end of file