robofish/rm_balance
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

生成bsp

Browse Source
This commit is contained in:
Robofish 2026-01-02 23:29:19 +08:00
parent 3dcf929fc0
commit 00cfee37c7
41 changed files with 4195 additions and 129 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

BIN
.DS_Store vendored
View File

Binary file not shown.

27
CMakeLists.txt
View File

@ -45,11 +45,38 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
# Add sources to executable # Add sources to executable
target_sources(${CMAKE_PROJECT_NAME} PRIVATE target_sources(${CMAKE_PROJECT_NAME} PRIVATE
# Add user sources here # 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 # Add include paths
target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE
# Add user defined include paths # Add user defined include paths
User
) )
# Add project symbols (macros) # Add project symbols (macros)

256
Core/Src/freertos.c
View File

@ -1,129 +1,127 @@
/* USER CODE BEGIN Header */ /* USER CODE BEGIN Header */
/** /**
****************************************************************************** ******************************************************************************
* File Name : freertos.c * File Name : freertos.c
* Description : Code for freertos applications * Description : Code for freertos applications
****************************************************************************** ******************************************************************************
* @attention * @attention
* *
* Copyright (c) 2026 STMicroelectronics. * Copyright (c) 2026 STMicroelectronics.
* All rights reserved. * All rights reserved.
* *
* This software is licensed under terms that can be found in the LICENSE file * This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component. * in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS. * If no LICENSE file comes with this software, it is provided AS-IS.
* *
****************************************************************************** ******************************************************************************
*/ */
/* USER CODE END Header */ /* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/ /* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "task.h" #include "task.h"
#include "main.h" #include "main.h"
#include "cmsis_os.h" #include "cmsis_os.h"
/* Private includes ----------------------------------------------------------*/ /* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */ /* USER CODE BEGIN Includes */
/* USER CODE END Includes */ #include "task/user_task.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */ /* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
/* USER CODE END PTD */
/* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */ /* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */
/* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */ /* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */
/* USER CODE END PM */
/* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */ /* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */
/* USER CODE END Variables */
/* Definitions for defaultTask */ /* USER CODE END Variables */
osThreadId_t defaultTaskHandle; /* Definitions for defaultTask */
const osThreadAttr_t defaultTask_attributes = { osThreadId_t defaultTaskHandle;
.name = "defaultTask", const osThreadAttr_t defaultTask_attributes = {
.stack_size = 128 * 4, .name = "defaultTask",
.priority = (osPriority_t) osPriorityNormal, .stack_size = 128 * 4,
}; .priority = (osPriority_t) osPriorityNormal,
};
/* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */ /* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */
/* USER CODE END FunctionPrototypes */
/* USER CODE END FunctionPrototypes */
void StartDefaultTask(void *argument);
void StartDefaultTask(void *argument);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
/**
* @brief FreeRTOS initialization /**
* @param None * @brief FreeRTOS initialization
* @retval None * @param None
*/ * @retval None
void MX_FREERTOS_Init(void) { */
/* USER CODE BEGIN Init */ void MX_FREERTOS_Init(void) {
/* USER CODE BEGIN Init */
/* USER CODE END Init */
/* USER CODE END Init */
/* USER CODE BEGIN RTOS_MUTEX */
/* add mutexes, ... */ /* USER CODE BEGIN RTOS_MUTEX */
/* USER CODE END RTOS_MUTEX */ /* add mutexes, ... */
/* USER CODE END RTOS_MUTEX */
/* USER CODE BEGIN RTOS_SEMAPHORES */
/* add semaphores, ... */ /* USER CODE BEGIN RTOS_SEMAPHORES */
/* USER CODE END RTOS_SEMAPHORES */ /* add semaphores, ... */
/* USER CODE END RTOS_SEMAPHORES */
/* USER CODE BEGIN RTOS_TIMERS */
/* start timers, add new ones, ... */ /* USER CODE BEGIN RTOS_TIMERS */
/* USER CODE END RTOS_TIMERS */ /* start timers, add new ones, ... */
/* USER CODE END RTOS_TIMERS */
/* USER CODE BEGIN RTOS_QUEUES */
/* add queues, ... */ /* USER CODE BEGIN RTOS_QUEUES */
/* USER CODE END RTOS_QUEUES */ /* add queues, ... */
/* USER CODE END RTOS_QUEUES */
/* Create the thread(s) */
/* creation of defaultTask */ /* Create the thread(s) */
defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes); /* creation of defaultTask */
defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */ /* USER CODE BEGIN RTOS_THREADS */
/* USER CODE END RTOS_THREADS */ /* add threads, ... */
osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务
/* USER CODE BEGIN RTOS_EVENTS */ /* USER CODE END RTOS_THREADS */
/* add events, ... */
/* USER CODE END RTOS_EVENTS */ /* USER CODE BEGIN RTOS_EVENTS */
/* add events, ... */
} /* USER CODE END RTOS_EVENTS */
/* USER CODE BEGIN Header_StartDefaultTask */ }
/**
* @brief Function implementing the defaultTask thread. /* USER CODE BEGIN Header_StartDefaultTask */
* @param argument: Not used /**
* @retval None * @brief Function implementing the defaultTask thread.
*/ * @param argument: Not used
/* USER CODE END Header_StartDefaultTask */ * @retval None
void StartDefaultTask(void *argument) */
{ /* USER CODE END Header_StartDefaultTask */
/* USER CODE BEGIN StartDefaultTask */ void StartDefaultTask(void *argument)
/* Infinite loop */ {
for(;;) /* USER CODE BEGIN StartDefaultTask */
{ osThreadTerminate(osThreadGetId());
osDelay(1); /* USER CODE END StartDefaultTask */
} }
/* USER CODE END StartDefaultTask */
} /* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */
/* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */ /* USER CODE END Application */
/* USER CODE END Application */

28
User/bsp/bsp.h Normal file
View File

@ -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

110
User/bsp/bsp_config.yaml Normal file
View File

@ -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

576
User/bsp/fdcan.c Normal file
View File

@ -0,0 +1,576 @@
/* Includes ----------------------------------------------------------------- */
#include "fdcan.h"
#include "bsp/fdcan.h"
#include "bsp/bsp.h"
#include <fdcan.h>
#include <cmsis_os2.h>
#include <string.h>
/* 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);
}
/* */

137
User/bsp/fdcan.h Normal file
View File

@ -0,0 +1,137 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#include "bsp/bsp.h"
#include "bsp/mm.h"
#include <cmsis_os.h>
/* USER INCLUDE BEGIN */
#include <fdcan.h>
/* 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

82
User/bsp/flash.c Normal file
View File

@ -0,0 +1,82 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp/flash.h"
#include <main.h>
#include <string.h>
/* 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, &sector_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);
}

46
User/bsp/flash.h Normal file
View File

@ -0,0 +1,46 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------ */
#include <main.h>
#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

123
User/bsp/gpio.c Normal file
View File

@ -0,0 +1,123 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp/gpio.h"
#include <gpio.h>
#include <main.h>
/* 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;
}

56
User/bsp/gpio.h Normal file
View File

@ -0,0 +1,56 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
#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

30
User/bsp/mm.c Normal file
View File

@ -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 */

32
User/bsp/mm.h Normal file
View File

@ -0,0 +1,32 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stddef.h>
#include <stdint.h>
/* 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

115
User/bsp/pwm.c Normal file
View File

@ -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;
}

53
User/bsp/pwm.h Normal file
View File

@ -0,0 +1,53 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#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

181
User/bsp/spi.c Normal file
View File

@ -0,0 +1,181 @@
/* Includes ----------------------------------------------------------------- */
#include <spi.h>
#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, &reg, 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, &reg, 1u, true);
return BSP_SPI_Transmit(spi, data, size, true);
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

70
User/bsp/spi.h Normal file
View File

@ -0,0 +1,70 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <spi.h>
#include <stdint.h>
#include <stdbool.h>
#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

81
User/bsp/time.c Normal file
View File

@ -0,0 +1,81 @@
/* Includes ----------------------------------------------------------------- */
#include "bsp/time.h"
#include "bsp.h"
#include <cmsis_os2.h>
#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 */

43
User/bsp/time.h Normal file
View File

@ -0,0 +1,43 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#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

159
User/bsp/uart.c Normal file
View File

@ -0,0 +1,159 @@
/* Includes ----------------------------------------------------------------- */
#include <usart.h>
#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 */

69
User/bsp/uart.h Normal file
View File

@ -0,0 +1,69 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <usart.h>
#include <stdint.h>
#include <stdbool.h>
#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

417
User/component/ahrs.c Normal file
View File

@ -0,0 +1,417 @@
/*
开源的AHRS算法。
MadgwickAHRS
*/
#include "ahrs.h"
#include <string.h>
#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 */

114
User/component/ahrs.h Normal file
View File

@ -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

7
User/component/component_config.yaml Normal file
View File

@ -0,0 +1,7 @@
ahrs:
dependencies:
- component/user_math.h
enabled: true
user_math:
dependencies: []
enabled: true

134
User/component/user_math.c Normal file
View File

@ -0,0 +1,134 @@
/*
自定义的数学运算。
*/
#include "user_math.h"
#include <string.h>
/* 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 */

179
User/component/user_math.h Normal file
View File

@ -0,0 +1,179 @@
/*
自定义的数学运算。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <float.h>
#include <math.h>
#include <stdbool.h>
#include <stdint.h>
#include <stddef.h>
/* 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 */

381
User/device/bmi088.c Normal file
View File

@ -0,0 +1,381 @@
/*
BMI088 陀螺仪+加速度计传感器。
*/
/* Includes ----------------------------------------------------------------- */
#include "bmi088.h"
#include <cmsis_os2.h>
#include <gpio.h>
#include <stdbool.h>
#include <string.h>
#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;
}

81
User/device/bmi088.h Normal file
View File

@ -0,0 +1,81 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdbool.h>
#include <stdint.h>
#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

51
User/device/device.h Normal file
View File

@ -0,0 +1,51 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stdint.h>
/* 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

15
User/device/device_config.yaml Normal file
View File

@ -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

169
User/device/dr16.c Normal file
View File

@ -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 <string.h>
#include <stdbool.h>
/* 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 */

117
User/device/dr16.h Normal file
View File

@ -0,0 +1,117 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#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

52
User/device/motor.c Normal file
View File

@ -0,0 +1,52 @@
/*
电机通用函数
*/
/* Includes ----------------------------------------------------------------- */
#include "motor.h"
#include <string.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 -------------------------------------------------------- */
/* 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;
}

68
User/device/motor.h Normal file
View File

@ -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

38
User/module/config.c Normal file
View File

@ -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;
}

38
User/module/config.h Normal file
View File

@ -0,0 +1,38 @@
/*
* 配置相关
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include <stdbool.h>
/**
* @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

44
User/task/ai.c Normal file
View File

@ -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); /* 运行结束，等待下一次唤醒 */
}
}

7
User/task/config.yaml Normal file
View File

@ -0,0 +1,7 @@
- delay: 0
description: ''
freq_control: true
frequency: 500.0
function: Task_ai
name: ai
stack: 256

42
User/task/init.c Normal file
View File

@ -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()); // 任务完成后结束自身
}

16
User/task/user_task.c Normal file
View File

@ -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,
};

80
User/task/user_task.h Normal file
View File

@ -0,0 +1,80 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <cmsis_os2.h>
#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