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

太扯了

Browse Source
This commit is contained in:
Robofish 2026-01-07 02:23:52 +08:00
parent e0b2485aaf
commit 0a9d6f80f4
85 changed files with 463 additions and 8569 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.

40
.mxproject
View File

File diff suppressed because one or more lines are too long

6
.settings/bundles-lock.store.json
View File

@ -117,7 +117,7 @@
"selected_by": [
{
"name": "st-arm-clangd",
"version": "19.1.2+st.3"
"version": "^19.1.2+st.3"
}
]
},
@ -128,7 +128,7 @@
"selected_by": [
{
"name": "st-arm-clangd",
"version": "19.1.2+st.3"
"version": "^19.1.2+st.3"
}
]
},
@ -139,7 +139,7 @@
"selected_by": [
{
"name": "st-arm-clangd",
"version": "19.1.2+st.3"
"version": "^19.1.2+st.3"
}
]
}

2
.settings/bundles.store.json
View File

@ -14,7 +14,7 @@
},
{
"name": "st-arm-clangd",
"version": "19.1.2+st.3"
"version": "^19.1.2+st.3"
}
]
}

41
CMakeLists.txt
View File

@ -45,52 +45,11 @@ target_link_directories(${CMAKE_PROJECT_NAME} PRIVATE
# Add sources to executable
target_sources(${CMAKE_PROJECT_NAME} PRIVATE
# Add user sources here
# User/bsp sources
User/bsp/fdcan.c
User/bsp/flash.c
User/bsp/gpio.c
User/bsp/mm.c
User/bsp/pwm.c
User/bsp/spi.c
User/bsp/time.c
User/bsp/uart.c
# User/component sources
User/component/ahrs.c
User/component/crc16.c
User/component/crc8.c
User/component/error_detect.c
User/component/filter.c
User/component/freertos_cli.c
User/component/limiter.c
User/component/pid.c
User/component/user_math.c
# User/device sources
User/device/bmi088.c
User/device/buzzer.c
User/device/dr16.c
User/device/motor.c
User/device/motor_dm.c
User/device/motor_lk.c
User/device/motor_lz.c
User/device/motor_rm.c
# User/module sources
User/module/config.c
# User/task sources
User/task/ai.c
User/task/atti_esti.c
User/task/init.c
User/task/rc.c
User/task/user_task.c
)
# Add include paths
target_include_directories(${CMAKE_PROJECT_NAME} PRIVATE
# Add user defined include paths
User
)
# Add project symbols (macros)

2
Core/Inc/FreeRTOSConfig.h
View File

@ -68,7 +68,7 @@
#define configTICK_RATE_HZ ((TickType_t)1000)
#define configMAX_PRIORITIES ( 56 )
#define configMINIMAL_STACK_SIZE ((uint16_t)128)
#define configTOTAL_HEAP_SIZE ((size_t)0x10000)
#define configTOTAL_HEAP_SIZE ((size_t)15360)
#define configMAX_TASK_NAME_LEN ( 16 )
#define configUSE_TRACE_FACILITY 1
#define configUSE_16_BIT_TICKS 0

12
Core/Inc/main.h
View File

@ -59,18 +59,22 @@ void Error_Handler(void);
/* Private defines -----------------------------------------------------------*/
#define POWER_24V_2_Pin GPIO_PIN_13
#define POWER_24V_2_GPIO_Port GPIOC
#define POWER_24_EN__Pin GPIO_PIN_14
#define POWER_24_EN__GPIO_Port GPIOC
#define POWER_5V_Pin GPIO_PIN_15
#define POWER_5V_GPIO_Port GPIOC
#define ACC_CS_Pin GPIO_PIN_0
#define ACC_CS_GPIO_Port GPIOC
#define ACCL_CS_Pin GPIO_PIN_0
#define ACCL_CS_GPIO_Port GPIOC
#define GYRO_CS_Pin GPIO_PIN_3
#define GYRO_CS_GPIO_Port GPIOC
#define ACC_INT_Pin GPIO_PIN_10
#define ACC_INT_GPIO_Port GPIOE
#define ACCL_INT_Pin GPIO_PIN_10
#define ACCL_INT_GPIO_Port GPIOE
#define ACCL_INT_EXTI_IRQn EXTI15_10_IRQn
#define W25Q64_CS_Pin GPIO_PIN_11
#define W25Q64_CS_GPIO_Port GPIOE
#define GYRO_INT_Pin GPIO_PIN_12
#define GYRO_INT_GPIO_Port GPIOE
#define GYRO_INT_EXTI_IRQn EXTI15_10_IRQn
#define LCD_CS_Pin GPIO_PIN_15
#define LCD_CS_GPIO_Port GPIOE
#define LCD_BLK_Pin GPIO_PIN_10

52
Core/Inc/octospi.h Normal file
View File

@ -0,0 +1,52 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file octospi.h
* @brief This file contains all the function prototypes for
* the octospi.c file
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Define to prevent recursive inclusion -------------------------------------*/
#ifndef __OCTOSPI_H__
#define __OCTOSPI_H__
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ------------------------------------------------------------------*/
#include "main.h"
/* USER CODE BEGIN Includes */
/* USER CODE END Includes */
extern OSPI_HandleTypeDef hospi1;
/* USER CODE BEGIN Private defines */
/* USER CODE END Private defines */
void MX_OCTOSPI1_Init(void);
/* USER CODE BEGIN Prototypes */
/* USER CODE END Prototypes */
#ifdef __cplusplus
}
#endif
#endif /* __OCTOSPI_H__ */

2
Core/Inc/stm32h7xx_hal_conf.h
View File

@ -58,7 +58,7 @@
/* #define HAL_GFXMMU_MODULE_ENABLED */
/* #define HAL_JPEG_MODULE_ENABLED */
/* #define HAL_OPAMP_MODULE_ENABLED */
/* #define HAL_OSPI_MODULE_ENABLED */
#define HAL_OSPI_MODULE_ENABLED
/* #define HAL_I2S_MODULE_ENABLED */
/* #define HAL_SMBUS_MODULE_ENABLED */
/* #define HAL_IWDG_MODULE_ENABLED */

3
Core/Inc/stm32h7xx_it.h
View File

@ -63,11 +63,11 @@ void FDCAN1_IT0_IRQHandler(void);
void FDCAN2_IT0_IRQHandler(void);
void FDCAN1_IT1_IRQHandler(void);
void FDCAN2_IT1_IRQHandler(void);
void TIM4_IRQHandler(void);
void SPI2_IRQHandler(void);
void USART1_IRQHandler(void);
void USART2_IRQHandler(void);
void USART3_IRQHandler(void);
void EXTI15_10_IRQHandler(void);
void DMA1_Stream7_IRQHandler(void);
void UART5_IRQHandler(void);
void DMA2_Stream0_IRQHandler(void);
@ -83,6 +83,7 @@ void BDMA_Channel0_IRQHandler(void);
void USART10_IRQHandler(void);
void FDCAN3_IT0_IRQHandler(void);
void FDCAN3_IT1_IRQHandler(void);
void TIM23_IRQHandler(void);
/* USER CODE BEGIN EFP */
/* USER CODE END EFP */

12
Core/Src/fdcan.c
View File

@ -40,9 +40,9 @@ void MX_FDCAN1_Init(void)
/* USER CODE END FDCAN1_Init 1 */
hfdcan1.Instance = FDCAN1;
hfdcan1.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
hfdcan1.Init.FrameFormat = FDCAN_FRAME_FD_BRS;
hfdcan1.Init.Mode = FDCAN_MODE_NORMAL;
hfdcan1.Init.AutoRetransmission = ENABLE;
hfdcan1.Init.AutoRetransmission = DISABLE;
hfdcan1.Init.TransmitPause = DISABLE;
hfdcan1.Init.ProtocolException = DISABLE;
hfdcan1.Init.NominalPrescaler = 5;
@ -88,9 +88,9 @@ void MX_FDCAN2_Init(void)
/* USER CODE END FDCAN2_Init 1 */
hfdcan2.Instance = FDCAN2;
hfdcan2.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
hfdcan2.Init.FrameFormat = FDCAN_FRAME_FD_BRS;
hfdcan2.Init.Mode = FDCAN_MODE_NORMAL;
hfdcan2.Init.AutoRetransmission = ENABLE;
hfdcan2.Init.AutoRetransmission = DISABLE;
hfdcan2.Init.TransmitPause = DISABLE;
hfdcan2.Init.ProtocolException = DISABLE;
hfdcan2.Init.NominalPrescaler = 5;
@ -136,9 +136,9 @@ void MX_FDCAN3_Init(void)
/* USER CODE END FDCAN3_Init 1 */
hfdcan3.Instance = FDCAN3;
hfdcan3.Init.FrameFormat = FDCAN_FRAME_CLASSIC;
hfdcan3.Init.FrameFormat = FDCAN_FRAME_FD_BRS;
hfdcan3.Init.Mode = FDCAN_MODE_NORMAL;
hfdcan3.Init.AutoRetransmission = ENABLE;
hfdcan3.Init.AutoRetransmission = DISABLE;
hfdcan3.Init.TransmitPause = DISABLE;
hfdcan3.Init.ProtocolException = DISABLE;
hfdcan3.Init.NominalPrescaler = 5;

8
Core/Src/freertos.c
View File

@ -26,7 +26,6 @@
/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "task/user_task.h"
/* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/
@ -97,7 +96,6 @@ void MX_FREERTOS_Init(void) {
/* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */
osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务
/* USER CODE END RTOS_THREADS */
/* USER CODE BEGIN RTOS_EVENTS */
@ -116,7 +114,11 @@ void MX_FREERTOS_Init(void) {
void StartDefaultTask(void *argument)
{
/* USER CODE BEGIN StartDefaultTask */
osThreadTerminate(osThreadGetId());
/* Infinite loop */
for(;;)
{
osDelay(1);
}
/* USER CODE END StartDefaultTask */
}

20
Core/Src/gpio.c
View File

@ -53,10 +53,10 @@ void MX_GPIO_Init(void)
__HAL_RCC_GPIOD_CLK_ENABLE();
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOC, POWER_24V_2_Pin|POWER_5V_Pin|ACC_CS_Pin|GYRO_CS_Pin, GPIO_PIN_SET);
HAL_GPIO_WritePin(GPIOC, POWER_24V_2_Pin|POWER_24_EN__Pin, GPIO_PIN_RESET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOC, GPIO_PIN_14, GPIO_PIN_RESET);
HAL_GPIO_WritePin(GPIOC, POWER_5V_Pin|ACCL_CS_Pin|GYRO_CS_Pin, GPIO_PIN_SET);
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOE, W25Q64_CS_Pin|LCD_CS_Pin, GPIO_PIN_RESET);
@ -64,22 +64,22 @@ void MX_GPIO_Init(void)
/*Configure GPIO pin Output Level */
HAL_GPIO_WritePin(GPIOB, LCD_BLK_Pin|LCD_RES_Pin, GPIO_PIN_RESET);
/*Configure GPIO pins : POWER_24V_2_Pin PC14 POWER_5V_Pin */
GPIO_InitStruct.Pin = POWER_24V_2_Pin|GPIO_PIN_14|POWER_5V_Pin;
/*Configure GPIO pins : POWER_24V_2_Pin POWER_24_EN__Pin POWER_5V_Pin */
GPIO_InitStruct.Pin = POWER_24V_2_Pin|POWER_24_EN__Pin|POWER_5V_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_LOW;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pins : ACC_CS_Pin GYRO_CS_Pin */
GPIO_InitStruct.Pin = ACC_CS_Pin|GYRO_CS_Pin;
/*Configure GPIO pins : ACCL_CS_Pin GYRO_CS_Pin */
GPIO_InitStruct.Pin = ACCL_CS_Pin|GYRO_CS_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_OUTPUT_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
HAL_GPIO_Init(GPIOC, &GPIO_InitStruct);
/*Configure GPIO pins : ACC_INT_Pin GYRO_INT_Pin */
GPIO_InitStruct.Pin = ACC_INT_Pin|GYRO_INT_Pin;
/*Configure GPIO pins : ACCL_INT_Pin GYRO_INT_Pin */
GPIO_InitStruct.Pin = ACCL_INT_Pin|GYRO_INT_Pin;
GPIO_InitStruct.Mode = GPIO_MODE_IT_RISING;
GPIO_InitStruct.Pull = GPIO_NOPULL;
HAL_GPIO_Init(GPIOE, &GPIO_InitStruct);
@ -107,6 +107,10 @@ void MX_GPIO_Init(void)
/*AnalogSwitch Config */
HAL_SYSCFG_AnalogSwitchConfig(SYSCFG_SWITCH_PC3, SYSCFG_SWITCH_PC3_CLOSE);
/* EXTI interrupt init*/
HAL_NVIC_SetPriority(EXTI15_10_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(EXTI15_10_IRQn);
}
/* USER CODE BEGIN 2 */

6
Core/Src/main.c
View File

@ -24,6 +24,7 @@
#include "dac.h"
#include "dma.h"
#include "fdcan.h"
#include "octospi.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
@ -121,6 +122,7 @@ int main(void)
MX_DAC1_Init();
MX_USB_OTG_HS_PCD_Init();
MX_UART5_Init();
MX_OCTOSPI1_Init();
/* USER CODE BEGIN 2 */
/* USER CODE END 2 */
@ -237,7 +239,7 @@ void PeriphCommonClock_Config(void)
/**
* @brief Period elapsed callback in non blocking mode
* @note This function is called when TIM4 interrupt took place, inside
* @note This function is called when TIM23 interrupt took place, inside
* HAL_TIM_IRQHandler(). It makes a direct call to HAL_IncTick() to increment
* a global variable "uwTick" used as application time base.
* @param htim : TIM handle
@ -248,7 +250,7 @@ void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
/* USER CODE BEGIN Callback 0 */
/* USER CODE END Callback 0 */
if (htim->Instance == TIM4)
if (htim->Instance == TIM23)
{
HAL_IncTick();
}

182
Core/Src/octospi.c Normal file
View File

@ -0,0 +1,182 @@
/* USER CODE BEGIN Header */
/**
******************************************************************************
* @file octospi.c
* @brief This file provides code for the configuration
* of the OCTOSPI instances.
******************************************************************************
* @attention
*
* Copyright (c) 2026 STMicroelectronics.
* All rights reserved.
*
* This software is licensed under terms that can be found in the LICENSE file
* in the root directory of this software component.
* If no LICENSE file comes with this software, it is provided AS-IS.
*
******************************************************************************
*/
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "octospi.h"
/* USER CODE BEGIN 0 */
/* USER CODE END 0 */
OSPI_HandleTypeDef hospi1;
/* OCTOSPI1 init function */
void MX_OCTOSPI1_Init(void)
{
/* USER CODE BEGIN OCTOSPI1_Init 0 */
/* USER CODE END OCTOSPI1_Init 0 */
OSPIM_CfgTypeDef sOspiManagerCfg = {0};
/* USER CODE BEGIN OCTOSPI1_Init 1 */
/* USER CODE END OCTOSPI1_Init 1 */
hospi1.Instance = OCTOSPI1;
hospi1.Init.FifoThreshold = 1;
hospi1.Init.DualQuad = HAL_OSPI_DUALQUAD_DISABLE;
hospi1.Init.MemoryType = HAL_OSPI_MEMTYPE_MICRON;
hospi1.Init.DeviceSize = 32;
hospi1.Init.ChipSelectHighTime = 1;
hospi1.Init.FreeRunningClock = HAL_OSPI_FREERUNCLK_DISABLE;
hospi1.Init.ClockMode = HAL_OSPI_CLOCK_MODE_0;
hospi1.Init.WrapSize = HAL_OSPI_WRAP_NOT_SUPPORTED;
hospi1.Init.ClockPrescaler = 1;
hospi1.Init.SampleShifting = HAL_OSPI_SAMPLE_SHIFTING_NONE;
hospi1.Init.DelayHoldQuarterCycle = HAL_OSPI_DHQC_DISABLE;
hospi1.Init.ChipSelectBoundary = 0;
hospi1.Init.DelayBlockBypass = HAL_OSPI_DELAY_BLOCK_BYPASSED;
hospi1.Init.MaxTran = 0;
hospi1.Init.Refresh = 0;
if (HAL_OSPI_Init(&hospi1) != HAL_OK)
{
Error_Handler();
}
sOspiManagerCfg.ClkPort = 1;
sOspiManagerCfg.IOLowPort = HAL_OSPIM_IOPORT_1_LOW;
if (HAL_OSPIM_Config(&hospi1, &sOspiManagerCfg, HAL_OSPI_TIMEOUT_DEFAULT_VALUE) != HAL_OK)
{
Error_Handler();
}
/* USER CODE BEGIN OCTOSPI1_Init 2 */
/* USER CODE END OCTOSPI1_Init 2 */
}
void HAL_OSPI_MspInit(OSPI_HandleTypeDef* ospiHandle)
{
GPIO_InitTypeDef GPIO_InitStruct = {0};
RCC_PeriphCLKInitTypeDef PeriphClkInitStruct = {0};
if(ospiHandle->Instance==OCTOSPI1)
{
/* USER CODE BEGIN OCTOSPI1_MspInit 0 */
/* USER CODE END OCTOSPI1_MspInit 0 */
/** Initializes the peripherals clock
*/
PeriphClkInitStruct.PeriphClockSelection = RCC_PERIPHCLK_OSPI;
PeriphClkInitStruct.OspiClockSelection = RCC_OSPICLKSOURCE_D1HCLK;
if (HAL_RCCEx_PeriphCLKConfig(&PeriphClkInitStruct) != HAL_OK)
{
Error_Handler();
}
/* OCTOSPI1 clock enable */
__HAL_RCC_OCTOSPIM_CLK_ENABLE();
__HAL_RCC_OSPI1_CLK_ENABLE();
__HAL_RCC_GPIOA_CLK_ENABLE();
__HAL_RCC_GPIOB_CLK_ENABLE();
__HAL_RCC_GPIOD_CLK_ENABLE();
/**OCTOSPI1 GPIO Configuration
PA1 ------> OCTOSPIM_P1_IO3
PA3 ------> OCTOSPIM_P1_IO2
PB0 ------> OCTOSPIM_P1_IO1
PB2 ------> OCTOSPIM_P1_CLK
PD11 ------> OCTOSPIM_P1_IO0
*/
GPIO_InitStruct.Pin = GPIO_PIN_1;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF9_OCTOSPIM_P1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_PIN_3;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF6_OCTOSPIM_P1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_PIN_0;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF4_OCTOSPIM_P1;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_PIN_2;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF9_OCTOSPIM_P1;
HAL_GPIO_Init(GPIOB, &GPIO_InitStruct);
GPIO_InitStruct.Pin = GPIO_PIN_11;
GPIO_InitStruct.Mode = GPIO_MODE_AF_PP;
GPIO_InitStruct.Pull = GPIO_NOPULL;
GPIO_InitStruct.Speed = GPIO_SPEED_FREQ_VERY_HIGH;
GPIO_InitStruct.Alternate = GPIO_AF9_OCTOSPIM_P1;
HAL_GPIO_Init(GPIOD, &GPIO_InitStruct);
/* USER CODE BEGIN OCTOSPI1_MspInit 1 */
/* USER CODE END OCTOSPI1_MspInit 1 */
}
}
void HAL_OSPI_MspDeInit(OSPI_HandleTypeDef* ospiHandle)
{
if(ospiHandle->Instance==OCTOSPI1)
{
/* USER CODE BEGIN OCTOSPI1_MspDeInit 0 */
/* USER CODE END OCTOSPI1_MspDeInit 0 */
/* Peripheral clock disable */
__HAL_RCC_OCTOSPIM_CLK_DISABLE();
__HAL_RCC_OSPI1_CLK_DISABLE();
/**OCTOSPI1 GPIO Configuration
PA1 ------> OCTOSPIM_P1_IO3
PA3 ------> OCTOSPIM_P1_IO2
PB0 ------> OCTOSPIM_P1_IO1
PB2 ------> OCTOSPIM_P1_CLK
PD11 ------> OCTOSPIM_P1_IO0
*/
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_1|GPIO_PIN_3);
HAL_GPIO_DeInit(GPIOB, GPIO_PIN_0|GPIO_PIN_2);
HAL_GPIO_DeInit(GPIOD, GPIO_PIN_11);
/* USER CODE BEGIN OCTOSPI1_MspDeInit 1 */
/* USER CODE END OCTOSPI1_MspDeInit 1 */
}
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

63
Core/Src/stm32h7xx_hal_timebase_tim.c
View File

@ -25,12 +25,12 @@
/* Private define ------------------------------------------------------------*/
/* Private macro -------------------------------------------------------------*/
/* Private variables ---------------------------------------------------------*/
TIM_HandleTypeDef htim4;
TIM_HandleTypeDef htim23;
/* Private function prototypes -----------------------------------------------*/
/* Private functions ---------------------------------------------------------*/
/**
* @brief This function configures the TIM4 as a time base source.
* @brief This function configures the TIM23 as a time base source.
* The time source is configured to have 1ms time base with a dedicated
* Tick interrupt priority.
* @note This function is called automatically at the beginning of program after
@ -41,17 +41,17 @@ TIM_HandleTypeDef htim4;
HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
{
RCC_ClkInitTypeDef clkconfig;
uint32_t uwTimclock, uwAPB1Prescaler;
uint32_t uwTimclock;
uint32_t uwPrescalerValue;
uint32_t pFLatency;
/*Configure the TIM4 IRQ priority */
/*Configure the TIM23 IRQ priority */
if (TickPriority < (1UL << __NVIC_PRIO_BITS))
{
HAL_NVIC_SetPriority(TIM4_IRQn, TickPriority ,0);
HAL_NVIC_SetPriority(TIM23_IRQn, TickPriority ,0);
/* Enable the TIM4 global Interrupt */
HAL_NVIC_EnableIRQ(TIM4_IRQn);
/* Enable the TIM23 global Interrupt */
HAL_NVIC_EnableIRQ(TIM23_IRQn);
uwTickPrio = TickPriority;
}
else
@ -59,45 +59,36 @@ HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
return HAL_ERROR;
}
/* Enable TIM4 clock */
__HAL_RCC_TIM4_CLK_ENABLE();
/* Enable TIM23 clock */
__HAL_RCC_TIM23_CLK_ENABLE();
/* Get clock configuration */
HAL_RCC_GetClockConfig(&clkconfig, &pFLatency);
/* Get APB1 prescaler */
uwAPB1Prescaler = clkconfig.APB1CLKDivider;
/* Compute TIM4 clock */
if (uwAPB1Prescaler == RCC_HCLK_DIV1)
{
uwTimclock = HAL_RCC_GetPCLK1Freq();
}
else
{
uwTimclock = 2UL * HAL_RCC_GetPCLK1Freq();
}
/* Compute TIM23 clock */
uwTimclock = 2*HAL_RCC_GetPCLK2Freq();
/* Compute the prescaler value to have TIM4 counter clock equal to 1MHz */
/* Compute the prescaler value to have TIM23 counter clock equal to 1MHz */
uwPrescalerValue = (uint32_t) ((uwTimclock / 1000000U) - 1U);
/* Initialize TIM4 */
htim4.Instance = TIM4;
/* Initialize TIM23 */
htim23.Instance = TIM23;
/* Initialize TIMx peripheral as follow:
* Period = [(TIM4CLK/1000) - 1]. to have a (1/1000) s time base.
* Period = [(TIM23CLK/1000) - 1]. to have a (1/1000) s time base.
* Prescaler = (uwTimclock/1000000 - 1) to have a 1MHz counter clock.
* ClockDivision = 0
* Counter direction = Up
*/
htim4.Init.Period = (1000000U / 1000U) - 1U;
htim4.Init.Prescaler = uwPrescalerValue;
htim4.Init.ClockDivision = 0;
htim4.Init.CounterMode = TIM_COUNTERMODE_UP;
htim23.Init.Period = (1000000U / 1000U) - 1U;
htim23.Init.Prescaler = uwPrescalerValue;
htim23.Init.ClockDivision = 0;
htim23.Init.CounterMode = TIM_COUNTERMODE_UP;
if(HAL_TIM_Base_Init(&htim4) == HAL_OK)
if(HAL_TIM_Base_Init(&htim23) == HAL_OK)
{
/* Start the TIM time Base generation in interrupt mode */
return HAL_TIM_Base_Start_IT(&htim4);
return HAL_TIM_Base_Start_IT(&htim23);
}
/* Return function status */
@ -106,25 +97,25 @@ HAL_StatusTypeDef HAL_InitTick(uint32_t TickPriority)
/**
* @brief Suspend Tick increment.
* @note Disable the tick increment by disabling TIM4 update interrupt.
* @note Disable the tick increment by disabling TIM23 update interrupt.
* @param None
* @retval None
*/
void HAL_SuspendTick(void)
{
/* Disable TIM4 update Interrupt */
__HAL_TIM_DISABLE_IT(&htim4, TIM_IT_UPDATE);
/* Disable TIM23 update Interrupt */
__HAL_TIM_DISABLE_IT(&htim23, TIM_IT_UPDATE);
}
/**
* @brief Resume Tick increment.
* @note Enable the tick increment by Enabling TIM4 update interrupt.
* @note Enable the tick increment by Enabling TIM23 update interrupt.
* @param None
* @retval None
*/
void HAL_ResumeTick(void)
{
/* Enable TIM4 Update interrupt */
__HAL_TIM_ENABLE_IT(&htim4, TIM_IT_UPDATE);
/* Enable TIM23 Update interrupt */
__HAL_TIM_ENABLE_IT(&htim23, TIM_IT_UPDATE);
}

45
Core/Src/stm32h7xx_it.c
View File

@ -82,7 +82,7 @@ extern UART_HandleTypeDef huart2;
extern UART_HandleTypeDef huart3;
extern UART_HandleTypeDef huart10;
extern PCD_HandleTypeDef hpcd_USB_OTG_HS;
extern TIM_HandleTypeDef htim4;
extern TIM_HandleTypeDef htim23;
/* USER CODE BEGIN EV */
@ -340,20 +340,6 @@ void FDCAN2_IT1_IRQHandler(void)
/* USER CODE END FDCAN2_IT1_IRQn 1 */
}
/**
* @brief This function handles TIM4 global interrupt.
*/
void TIM4_IRQHandler(void)
{
/* USER CODE BEGIN TIM4_IRQn 0 */
/* USER CODE END TIM4_IRQn 0 */
HAL_TIM_IRQHandler(&htim4);
/* USER CODE BEGIN TIM4_IRQn 1 */
/* USER CODE END TIM4_IRQn 1 */
}
/**
* @brief This function handles SPI2 global interrupt.
*/
@ -410,6 +396,21 @@ void USART3_IRQHandler(void)
/* USER CODE END USART3_IRQn 1 */
}
/**
* @brief This function handles EXTI line[15:10] interrupts.
*/
void EXTI15_10_IRQHandler(void)
{
/* USER CODE BEGIN EXTI15_10_IRQn 0 */
/* USER CODE END EXTI15_10_IRQn 0 */
HAL_GPIO_EXTI_IRQHandler(ACCL_INT_Pin);
HAL_GPIO_EXTI_IRQHandler(GYRO_INT_Pin);
/* USER CODE BEGIN EXTI15_10_IRQn 1 */
/* USER CODE END EXTI15_10_IRQn 1 */
}
/**
* @brief This function handles DMA1 stream7 global interrupt.
*/
@ -620,6 +621,20 @@ void FDCAN3_IT1_IRQHandler(void)
/* USER CODE END FDCAN3_IT1_IRQn 1 */
}
/**
* @brief This function handles TIM23 global interrupt.
*/
void TIM23_IRQHandler(void)
{
/* USER CODE BEGIN TIM23_IRQn 0 */
/* USER CODE END TIM23_IRQn 0 */
HAL_TIM_IRQHandler(&htim23);
/* USER CODE BEGIN TIM23_IRQn 1 */
/* USER CODE END TIM23_IRQn 1 */
}
/* USER CODE BEGIN 1 */
/* USER CODE END 1 */

15
STM32H723XG_FLASH.ld
View File

@ -66,8 +66,8 @@ FLASH (rx) : ORIGIN = 0x8000000, LENGTH = 1024K
/* Highest address of the user mode stack */
_estack = ORIGIN(DTCMRAM) + LENGTH(DTCMRAM); /* end of RAM */
/* Generate a link error if heap and stack don't fit into RAM */
_Min_Heap_Size = 0x1000; /* required amount of heap */
_Min_Stack_Size = 0x2000; /* required amount of stack */
_Min_Heap_Size = 0x200; /* required amount of heap */
_Min_Stack_Size = 0x800; /* required amount of stack */
/* Define output sections */
SECTIONS
@ -229,17 +229,6 @@ SECTIONS
PROVIDE( __bss_start = __tbss_start );
PROVIDE( __bss_size = __bss_end - __bss_start );
/* DMA buffer section in RAM_D2 (SRAM1/2) for STM32H7 DMA access */
.dma_buffer (NOLOAD) : ALIGN(32)
{
. = ALIGN(32);
_sdma_buffer = .;
*(.dma_buffer)
*(.dma_buffer*)
. = ALIGN(32);
_edma_buffer = .;
} >RAM_D2
/* User_heap_stack section, used to check that there is enough RAM left */
._user_heap_stack (NOLOAD) :
{

28
User/bsp/bsp.h
View File

@ -1,28 +0,0 @@
#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
View File

@ -1,110 +0,0 @@
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

79
User/bsp/can.h
View File

@ -1,79 +0,0 @@
/**
* @file can.h
* @brief CAN兼容层 - 将CAN接口映射到FDCAN
* @note 本文件用于FDCAN兼容CAN接口，设备层代码可以继续使用BSP_CAN_xxx接口
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "bsp/fdcan.h"
/* 类型映射 */
typedef BSP_FDCAN_t BSP_CAN_t;
typedef BSP_FDCAN_Callback_t BSP_CAN_Callback_t;
typedef BSP_FDCAN_Format_t BSP_CAN_Format_t;
typedef BSP_FDCAN_FrameType_t BSP_CAN_FrameType_t;
typedef BSP_FDCAN_Message_t BSP_CAN_Message_t;
typedef BSP_FDCAN_StdDataFrame_t BSP_CAN_StdDataFrame_t;
typedef BSP_FDCAN_ExtDataFrame_t BSP_CAN_ExtDataFrame_t;
typedef BSP_FDCAN_RemoteFrame_t BSP_CAN_RemoteFrame_t;
typedef BSP_FDCAN_IdParser_t BSP_CAN_IdParser_t;
/* 常量映射 */
#define BSP_CAN_MAX_DLC BSP_FDCAN_MAX_DLC
#define BSP_CAN_DEFAULT_QUEUE_SIZE BSP_FDCAN_DEFAULT_QUEUE_SIZE
#define BSP_CAN_TIMEOUT_IMMEDIATE BSP_FDCAN_TIMEOUT_IMMEDIATE
#define BSP_CAN_TIMEOUT_FOREVER BSP_FDCAN_TIMEOUT_FOREVER
#define BSP_CAN_TX_QUEUE_SIZE BSP_FDCAN_TX_QUEUE_SIZE
/* 枚举值映射 */
#define BSP_CAN_1 BSP_FDCAN_1
#define BSP_CAN_2 BSP_FDCAN_2
#define BSP_CAN_3 BSP_FDCAN_3
#define BSP_CAN_NUM BSP_FDCAN_NUM
#define BSP_CAN_ERR BSP_FDCAN_ERR
#define BSP_CAN_FORMAT_STD_DATA BSP_FDCAN_FORMAT_STD_DATA
#define BSP_CAN_FORMAT_EXT_DATA BSP_FDCAN_FORMAT_EXT_DATA
#define BSP_CAN_FORMAT_STD_REMOTE BSP_FDCAN_FORMAT_STD_REMOTE
#define BSP_CAN_FORMAT_EXT_REMOTE BSP_FDCAN_FORMAT_EXT_REMOTE
#define BSP_CAN_FRAME_STD_DATA BSP_FDCAN_FRAME_STD_DATA
#define BSP_CAN_FRAME_EXT_DATA BSP_FDCAN_FRAME_EXT_DATA
#define BSP_CAN_FRAME_STD_REMOTE BSP_FDCAN_FRAME_STD_REMOTE
#define BSP_CAN_FRAME_EXT_REMOTE BSP_FDCAN_FRAME_EXT_REMOTE
/* 函数映射 */
#define BSP_CAN_Init() BSP_FDCAN_Init()
#define BSP_CAN_GetHandle(can) BSP_FDCAN_GetHandle(can)
#define BSP_CAN_RegisterCallback(can, type, callback) \
BSP_FDCAN_RegisterCallback(can, type, callback)
#define BSP_CAN_Transmit(can, format, id, data, dlc) \
BSP_FDCAN_Transmit(can, format, id, data, dlc)
#define BSP_CAN_TransmitStdDataFrame(can, frame) \
BSP_FDCAN_TransmitStdDataFrame(can, frame)
#define BSP_CAN_TransmitExtDataFrame(can, frame) \
BSP_FDCAN_TransmitExtDataFrame(can, frame)
#define BSP_CAN_TransmitRemoteFrame(can, frame) \
BSP_FDCAN_TransmitRemoteFrame(can, frame)
#define BSP_CAN_RegisterId(can, can_id, queue_size) \
BSP_FDCAN_RegisterId(can, can_id, queue_size)
#define BSP_CAN_GetMessage(can, can_id, msg, timeout) \
BSP_FDCAN_GetMessage(can, can_id, msg, timeout)
#define BSP_CAN_GetQueueCount(can, can_id) \
BSP_FDCAN_GetQueueCount(can, can_id)
#define BSP_CAN_FlushQueue(can, can_id) \
BSP_FDCAN_FlushQueue(can, can_id)
#define BSP_CAN_RegisterIdParser(parser) \
BSP_FDCAN_RegisterIdParser(parser)
#define BSP_CAN_ParseId(original_id, frame_type) \
BSP_FDCAN_ParseId(original_id, frame_type)
#ifdef __cplusplus
}
#endif

576
User/bsp/fdcan.c
View File

@ -1,576 +0,0 @@
/* 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
View File

@ -1,137 +0,0 @@
#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
View File

@ -1,82 +0,0 @@
/* 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
View File

@ -1,46 +0,0 @@
#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
View File

@ -1,123 +0,0 @@
/* 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
View File

@ -1,56 +0,0 @@
#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
View File

@ -1,30 +0,0 @@
/* 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
View File

@ -1,32 +0,0 @@
#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
View File

@ -1,115 +0,0 @@
/* 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
View File

@ -1,53 +0,0 @@
#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
View File

@ -1,181 +0,0 @@
/* 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
View File

@ -1,70 +0,0 @@
#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
View File

@ -1,81 +0,0 @@
/* 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
View File

@ -1,43 +0,0 @@
#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
View File

@ -1,159 +0,0 @@
/* 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
View File

@ -1,69 +0,0 @@
#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
View File

@ -1,417 +0,0 @@
/*
开源的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
View File

@ -1,114 +0,0 @@
/*
开源的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

31
User/component/component_config.yaml
View File

@ -1,31 +0,0 @@
ahrs:
dependencies:
- component/user_math.h
enabled: true
crc16:
dependencies: []
enabled: true
crc8:
dependencies: []
enabled: true
error_detect:
dependencies:
- bsp/mm
enabled: true
filter:
dependencies:
- component/ahrs
enabled: true
freertos_cli:
dependencies: []
enabled: true
limiter:
dependencies: []
enabled: true
pid:
dependencies:
- component/filter
enabled: true
user_math:
dependencies: []
enabled: true

62
User/component/crc16.c
View File

@ -1,62 +0,0 @@
#include "crc16.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
static const uint16_t crc16_tab[256] = {
0x0000, 0x1189, 0x2312, 0x329b, 0x4624, 0x57ad, 0x6536, 0x74bf, 0x8c48,
0x9dc1, 0xaf5a, 0xbed3, 0xca6c, 0xdbe5, 0xe97e, 0xf8f7, 0x1081, 0x0108,
0x3393, 0x221a, 0x56a5, 0x472c, 0x75b7, 0x643e, 0x9cc9, 0x8d40, 0xbfdb,
0xae52, 0xdaed, 0xcb64, 0xf9ff, 0xe876, 0x2102, 0x308b, 0x0210, 0x1399,
0x6726, 0x76af, 0x4434, 0x55bd, 0xad4a, 0xbcc3, 0x8e58, 0x9fd1, 0xeb6e,
0xfae7, 0xc87c, 0xd9f5, 0x3183, 0x200a, 0x1291, 0x0318, 0x77a7, 0x662e,
0x54b5, 0x453c, 0xbdcb, 0xac42, 0x9ed9, 0x8f50, 0xfbef, 0xea66, 0xd8fd,
0xc974, 0x4204, 0x538d, 0x6116, 0x709f, 0x0420, 0x15a9, 0x2732, 0x36bb,
0xce4c, 0xdfc5, 0xed5e, 0xfcd7, 0x8868, 0x99e1, 0xab7a, 0xbaf3, 0x5285,
0x430c, 0x7197, 0x601e, 0x14a1, 0x0528, 0x37b3, 0x263a, 0xdecd, 0xcf44,
0xfddf, 0xec56, 0x98e9, 0x8960, 0xbbfb, 0xaa72, 0x6306, 0x728f, 0x4014,
0x519d, 0x2522, 0x34ab, 0x0630, 0x17b9, 0xef4e, 0xfec7, 0xcc5c, 0xddd5,
0xa96a, 0xb8e3, 0x8a78, 0x9bf1, 0x7387, 0x620e, 0x5095, 0x411c, 0x35a3,
0x242a, 0x16b1, 0x0738, 0xffcf, 0xee46, 0xdcdd, 0xcd54, 0xb9eb, 0xa862,
0x9af9, 0x8b70, 0x8408, 0x9581, 0xa71a, 0xb693, 0xc22c, 0xd3a5, 0xe13e,
0xf0b7, 0x0840, 0x19c9, 0x2b52, 0x3adb, 0x4e64, 0x5fed, 0x6d76, 0x7cff,
0x9489, 0x8500, 0xb79b, 0xa612, 0xd2ad, 0xc324, 0xf1bf, 0xe036, 0x18c1,
0x0948, 0x3bd3, 0x2a5a, 0x5ee5, 0x4f6c, 0x7df7, 0x6c7e, 0xa50a, 0xb483,
0x8618, 0x9791, 0xe32e, 0xf2a7, 0xc03c, 0xd1b5, 0x2942, 0x38cb, 0x0a50,
0x1bd9, 0x6f66, 0x7eef, 0x4c74, 0x5dfd, 0xb58b, 0xa402, 0x9699, 0x8710,
0xf3af, 0xe226, 0xd0bd, 0xc134, 0x39c3, 0x284a, 0x1ad1, 0x0b58, 0x7fe7,
0x6e6e, 0x5cf5, 0x4d7c, 0xc60c, 0xd785, 0xe51e, 0xf497, 0x8028, 0x91a1,
0xa33a, 0xb2b3, 0x4a44, 0x5bcd, 0x6956, 0x78df, 0x0c60, 0x1de9, 0x2f72,
0x3efb, 0xd68d, 0xc704, 0xf59f, 0xe416, 0x90a9, 0x8120, 0xb3bb, 0xa232,
0x5ac5, 0x4b4c, 0x79d7, 0x685e, 0x1ce1, 0x0d68, 0x3ff3, 0x2e7a, 0xe70e,
0xf687, 0xc41c, 0xd595, 0xa12a, 0xb0a3, 0x8238, 0x93b1, 0x6b46, 0x7acf,
0x4854, 0x59dd, 0x2d62, 0x3ceb, 0x0e70, 0x1ff9, 0xf78f, 0xe606, 0xd49d,
0xc514, 0xb1ab, 0xa022, 0x92b9, 0x8330, 0x7bc7, 0x6a4e, 0x58d5, 0x495c,
0x3de3, 0x2c6a, 0x1ef1, 0x0f78};
static inline uint16_t CRC16_Byte(uint16_t crc, const uint8_t data) {
return (crc >> 8) ^ crc16_tab[(crc ^ data) & 0xff];
}
uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc) {
while (len--) crc = CRC16_Byte(crc, *buf++);
return crc;
}
bool CRC16_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;
uint16_t expected = CRC16_Calc(buf, len - sizeof(uint16_t), CRC16_INIT);
return expected ==
((const uint16_t *)((const uint8_t *)buf +
(len % 2)))[len / sizeof(uint16_t) - 1];
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

30
User/component/crc16.h
View File

@ -1,30 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define CRC16_INIT 0XFFFF
uint16_t CRC16_Calc(const uint8_t *buf, size_t len, uint16_t crc);
bool CRC16_Verify(const uint8_t *buf, size_t len);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

52
User/component/crc8.c
View File

@ -1,52 +0,0 @@
#include "crc8.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
static const uint8_t crc8_tab[256] = {
0x00, 0x5e, 0xbc, 0xe2, 0x61, 0x3f, 0xdd, 0x83, 0xc2, 0x9c, 0x7e, 0x20,
0xa3, 0xfd, 0x1f, 0x41, 0x9d, 0xc3, 0x21, 0x7f, 0xfc, 0xa2, 0x40, 0x1e,
0x5f, 0x01, 0xe3, 0xbd, 0x3e, 0x60, 0x82, 0xdc, 0x23, 0x7d, 0x9f, 0xc1,
0x42, 0x1c, 0xfe, 0xa0, 0xe1, 0xbf, 0x5d, 0x03, 0x80, 0xde, 0x3c, 0x62,
0xbe, 0xe0, 0x02, 0x5c, 0xdf, 0x81, 0x63, 0x3d, 0x7c, 0x22, 0xc0, 0x9e,
0x1d, 0x43, 0xa1, 0xff, 0x46, 0x18, 0xfa, 0xa4, 0x27, 0x79, 0x9b, 0xc5,
0x84, 0xda, 0x38, 0x66, 0xe5, 0xbb, 0x59, 0x07, 0xdb, 0x85, 0x67, 0x39,
0xba, 0xe4, 0x06, 0x58, 0x19, 0x47, 0xa5, 0xfb, 0x78, 0x26, 0xc4, 0x9a,
0x65, 0x3b, 0xd9, 0x87, 0x04, 0x5a, 0xb8, 0xe6, 0xa7, 0xf9, 0x1b, 0x45,
0xc6, 0x98, 0x7a, 0x24, 0xf8, 0xa6, 0x44, 0x1a, 0x99, 0xc7, 0x25, 0x7b,
0x3a, 0x64, 0x86, 0xd8, 0x5b, 0x05, 0xe7, 0xb9, 0x8c, 0xd2, 0x30, 0x6e,
0xed, 0xb3, 0x51, 0x0f, 0x4e, 0x10, 0xf2, 0xac, 0x2f, 0x71, 0x93, 0xcd,
0x11, 0x4f, 0xad, 0xf3, 0x70, 0x2e, 0xcc, 0x92, 0xd3, 0x8d, 0x6f, 0x31,
0xb2, 0xec, 0x0e, 0x50, 0xaf, 0xf1, 0x13, 0x4d, 0xce, 0x90, 0x72, 0x2c,
0x6d, 0x33, 0xd1, 0x8f, 0x0c, 0x52, 0xb0, 0xee, 0x32, 0x6c, 0x8e, 0xd0,
0x53, 0x0d, 0xef, 0xb1, 0xf0, 0xae, 0x4c, 0x12, 0x91, 0xcf, 0x2d, 0x73,
0xca, 0x94, 0x76, 0x28, 0xab, 0xf5, 0x17, 0x49, 0x08, 0x56, 0xb4, 0xea,
0x69, 0x37, 0xd5, 0x8b, 0x57, 0x09, 0xeb, 0xb5, 0x36, 0x68, 0x8a, 0xd4,
0x95, 0xcb, 0x29, 0x77, 0xf4, 0xaa, 0x48, 0x16, 0xe9, 0xb7, 0x55, 0x0b,
0x88, 0xd6, 0x34, 0x6a, 0x2b, 0x75, 0x97, 0xc9, 0x4a, 0x14, 0xf6, 0xa8,
0x74, 0x2a, 0xc8, 0x96, 0x15, 0x4b, 0xa9, 0xf7, 0xb6, 0xe8, 0x0a, 0x54,
0xd7, 0x89, 0x6b, 0x35,
};
uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc) {
/* loop over the buffer data */
while (len-- > 0) crc = crc8_tab[(crc ^ *buf++) & 0xff];
return crc;
}
bool CRC8_Verify(const uint8_t *buf, size_t len) {
if (len < 2) return false;
uint8_t expected = CRC8_Calc(buf, len - sizeof(uint8_t), CRC8_INIT);
return expected == buf[len - sizeof(uint8_t)];
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

30
User/component/crc8.h
View File

@ -1,30 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdbool.h>
#include <stddef.h>
#include <stdint.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define CRC8_INIT 0xFF
uint8_t CRC8_Calc(const uint8_t *buf, size_t len, uint8_t crc);
bool CRC8_Verify(const uint8_t *buf, size_t len);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

67
User/component/error_detect.c
View File

@ -1,67 +0,0 @@
/*
错误检测。
*/
#include "error_detect.h"
#include <stddef.h>
#include <string.h>
#include "bsp/mm.h"
static ErrorDetect_t ged;
static bool inited = false;
int8_t ErrorDetect_Init(void) {
if (inited) return -1;
memset(&ged, 0x00, sizeof(ged));
for (uint8_t i = 0; i < ERROR_DETECT_UNIT_NUM; i++) {
ged.error[i].enable = true;
ged.error[i].priority = i;
ged.error[i].patient_lost = 500;
ged.error[i].patient_work = 500;
}
return 0;
}
void ErrorDetect_Processing(uint32_t sys_time) {
for (uint8_t i = 0; i < ERROR_DETECT_UNIT_NUM; i++) {
if (!ged.error[i].enable) continue;
if (sys_time - ged.error[i].showup > ged.error[i].patient_lost) {
ged.error[i].is_lost = true;
ged.error[i].found_lost = sys_time;
} else if (sys_time - ged.error[i].showup > ged.error[i].patient_lost) {
} else {
ged.error[i].cycle_time = ged.error[i].showup - ged.error[i].showup_last;
}
}
}
bool ErrorDetect_ErrorExist(ErrorDetect_Unit_t unit) {
if (unit == ERROR_DETECT_UNIT_NO_DEV) {
for (uint8_t i = ERROR_DETECT_UNIT_NUM; i > 0; i--) {
if (ged.error[i].error_exist) return true;
}
return false;
} else {
return ged.error[unit].error_exist;
}
}
ErrorDetect_Unit_t ErrorDetect_GetErrorUnit(void) {
for (uint8_t i = ERROR_DETECT_UNIT_NUM; i > 0; i--) {
if (ged.error[i].error_exist) return i;
}
return ERROR_DETECT_UNIT_NO_DEV;
}
const ErrorDetect_Error_t *ErrorDetect_GetDetail(ErrorDetect_Unit_t unit) {
return &ged.error[unit];
}
void ErrorDetect_Update(ErrorDetect_Unit_t unit, uint32_t time_current) {
ged.error[unit].showup = time_current;
}

82
User/component/error_detect.h
View File

@ -1,82 +0,0 @@
/*
错误检测。
*/
#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 */
typedef enum {
/* Low priority */
ERROR_DETECT_UNIT_NO_DEV = 0,
ERROR_DETECT_UNIT_REFEREE,
ERROR_DETECT_UNIT_CHASSIS_M1,
ERROR_DETECT_UNIT_CHASSIS_M2,
ERROR_DETECT_UNIT_CHASSIS_M3,
ERROR_DETECT_UNIT_CHASSIS_M4,
ERROR_DETECT_UNIT_TRIGGER,
ERROR_DETECT_UNIT_FEED,
ERROR_DETECT_UNIT_GIMBAL_YAW,
ERROR_DETECT_UNIT_GIMBAL_PIT,
ERROR_DETECT_UNIT_GYRO,
ERROR_DETECT_UNIT_ACCL,
ERROR_DETECT_UNIT_MAGN,
ERROR_DETECT_UNIT_DBUS,
ERROR_DETECT_UNIT_NUM,
/* High priority */
} ErrorDetect_Unit_t;
typedef struct {
bool enable;
uint8_t priority;
uint32_t patient_lost;
uint32_t patient_work;
uint32_t showup;
uint32_t showup_last;
uint32_t cycle_time;
uint32_t duration_lost;
uint32_t duration_work;
uint32_t found_lost;
bool error_exist;
bool is_lost;
uint8_t data_is_error;
} ErrorDetect_Error_t;
typedef struct {
ErrorDetect_Error_t error[ERROR_DETECT_UNIT_NUM];
} ErrorDetect_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
int8_t ErrorDetect_Init(void);
void ErrorDetect_Processing(uint32_t sys_time);
bool ErrorDetect_ErrorExist(ErrorDetect_Unit_t unit);
ErrorDetect_Unit_t ErrorDetect_GetErrorUnit(void);
const ErrorDetect_Error_t *ErrorDetect_GetDetail(ErrorDetect_Unit_t unit);
void ErrorDetect_Update(ErrorDetect_Unit_t unit, uint32_t time_current);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

185
User/component/filter.c
View File

@ -1,185 +0,0 @@
/*
各类滤波器。
*/
#include "filter.h"
#include "user_math.h"
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param cutoff_freq 截止频率
*/
void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
float cutoff_freq) {
if (f == NULL) return;
f->cutoff_freq = cutoff_freq;
f->delay_element_1 = 0.0f;
f->delay_element_2 = 0.0f;
if (f->cutoff_freq <= 0.0f) {
/* no filtering */
f->b0 = 1.0f;
f->b1 = 0.0f;
f->b2 = 0.0f;
f->a1 = 0.0f;
f->a2 = 0.0f;
return;
}
const float fr = sample_freq / f->cutoff_freq;
const float ohm = tanf(M_PI / fr);
const float c = 1.0f + 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm;
f->b0 = ohm * ohm / c;
f->b1 = 2.0f * f->b0;
f->b2 = f->b0;
f->a1 = 2.0f * (ohm * ohm - 1.0f) / c;
f->a2 = (1.0f - 2.0f * cosf(M_PI / 4.0f) * ohm + ohm * ohm) / c;
}
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample) {
if (f == NULL) return 0.0f;
/* do the filtering */
float delay_element_0 =
sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
if (isinf(delay_element_0)) {
/* don't allow bad values to propagate via the filter */
delay_element_0 = sample;
}
const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
f->delay_element_2 * f->b2;
f->delay_element_2 = f->delay_element_1;
f->delay_element_1 = delay_element_0;
/* return the value. Should be no need to check limits */
return output;
}
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample) {
if (f == NULL) return 0.0f;
const float dval = sample / (f->b0 + f->b1 + f->b2);
if (isfinite(dval)) {
f->delay_element_1 = dval;
f->delay_element_2 = dval;
} else {
f->delay_element_1 = sample;
f->delay_element_2 = sample;
}
return LowPassFilter2p_Apply(f, sample);
}
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param notch_freq 中心频率
* @param bandwidth 带宽
*/
void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
float bandwidth) {
if (f == NULL) return;
f->notch_freq = notch_freq;
f->bandwidth = bandwidth;
f->delay_element_1 = 0.0f;
f->delay_element_2 = 0.0f;
if (notch_freq <= 0.0f) {
/* no filtering */
f->b0 = 1.0f;
f->b1 = 0.0f;
f->b2 = 0.0f;
f->a1 = 0.0f;
f->a2 = 0.0f;
return;
}
const float alpha = tanf(M_PI * bandwidth / sample_freq);
const float beta = -cosf(M_2PI * notch_freq / sample_freq);
const float a0_inv = 1.0f / (alpha + 1.0f);
f->b0 = a0_inv;
f->b1 = 2.0f * beta * a0_inv;
f->b2 = a0_inv;
f->a1 = f->b1;
f->a2 = (1.0f - alpha) * a0_inv;
}
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
inline float NotchFilter_Apply(NotchFilter_t *f, float sample) {
if (f == NULL) return 0.0f;
/* Direct Form II implementation */
const float delay_element_0 =
sample - f->delay_element_1 * f->a1 - f->delay_element_2 * f->a2;
const float output = delay_element_0 * f->b0 + f->delay_element_1 * f->b1 +
f->delay_element_2 * f->b2;
f->delay_element_2 = f->delay_element_1;
f->delay_element_1 = delay_element_0;
return output;
}
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Reset(NotchFilter_t *f, float sample) {
if (f == NULL) return 0.0f;
float dval = sample;
if (fabsf(f->b0 + f->b1 + f->b2) > FLT_EPSILON) {
dval = dval / (f->b0 + f->b1 + f->b2);
}
f->delay_element_1 = dval;
f->delay_element_2 = dval;
return NotchFilter_Apply(f, sample);
}

120
User/component/filter.h
View File

@ -1,120 +0,0 @@
/*
各类滤波器。
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* 二阶低通滤波器 */
typedef struct {
float cutoff_freq; /* 截止频率 */
float a1;
float a2;
float b0;
float b1;
float b2;
float delay_element_1;
float delay_element_2;
} LowPassFilter2p_t;
/* 带阻滤波器 */
typedef struct {
float notch_freq; /* 阻止频率 */
float bandwidth; /* 带宽 */
float a1;
float a2;
float b0;
float b1;
float b2;
float delay_element_1;
float delay_element_2;
} NotchFilter_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param cutoff_freq 截止频率
*/
void LowPassFilter2p_Init(LowPassFilter2p_t *f, float sample_freq,
float cutoff_freq);
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Apply(LowPassFilter2p_t *f, float sample);
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float LowPassFilter2p_Reset(LowPassFilter2p_t *f, float sample);
/**
* @brief 初始化滤波器
*
* @param f 滤波器
* @param sample_freq 采样频率
* @param notch_freq 中心频率
* @param bandwidth 带宽
*/
void NotchFilter_Init(NotchFilter_t *f, float sample_freq, float notch_freq,
float bandwidth);
/**
* @brief 施加一次滤波计算
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Apply(NotchFilter_t *f, float sample);
/**
* @brief 重置滤波器
*
* @param f 滤波器
* @param sample 采样的值
* @return float 滤波后的值
*/
float NotchFilter_Reset(NotchFilter_t *f, float sample);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

352
User/component/freertos_cli.c
View File

@ -1,352 +0,0 @@
/*
* FreeRTOS+CLI V1.0.4
* Copyright (C) 2017 Amazon.com, Inc. or its affiliates. All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* http://www.FreeRTOS.org
* http://aws.amazon.com/freertos
*
* 1 tab == 4 spaces!
*/
/* Standard includes. */
#include <string.h>
#include <stdint.h>
/* FreeRTOS includes. */
#include "FreeRTOS.h"
#include "task.h"
/* Utils includes. */
#include "FreeRTOS_CLI.h"
/* If the application writer needs to place the buffer used by the CLI at a
fixed address then set configAPPLICATION_PROVIDES_cOutputBuffer to 1 in
FreeRTOSConfig.h, then declare an array with the following name and size in
one of the application files:
char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];
*/
#ifndef configAPPLICATION_PROVIDES_cOutputBuffer
#define configAPPLICATION_PROVIDES_cOutputBuffer 0
#endif
typedef struct xCOMMAND_INPUT_LIST
{
const CLI_Command_Definition_t *pxCommandLineDefinition;
struct xCOMMAND_INPUT_LIST *pxNext;
} CLI_Definition_List_Item_t;
/*
* The callback function that is executed when "help" is entered. This is the
* only default command that is always present.
*/
static BaseType_t prvHelpCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
/*
* Return the number of parameters that follow the command name.
*/
static int8_t prvGetNumberOfParameters( const char *pcCommandString );
/* The definition of the "help" command. This command is always at the front
of the list of registered commands. */
static const CLI_Command_Definition_t xHelpCommand =
{
"help",
"\r\nhelp:\r\n Lists all the registered commands\r\n\r\n",
prvHelpCommand,
0
};
/* The definition of the list of commands. Commands that are registered are
added to this list. */
static CLI_Definition_List_Item_t xRegisteredCommands =
{
&xHelpCommand, /* The first command in the list is always the help command, defined in this file. */
NULL /* The next pointer is initialised to NULL, as there are no other registered commands yet. */
};
/* A buffer into which command outputs can be written is declared here, rather
than in the command console implementation, to allow multiple command consoles
to share the same buffer. For example, an application may allow access to the
command interpreter by UART and by Ethernet. Sharing a buffer is done purely
to save RAM. Note, however, that the command console itself is not re-entrant,
so only one command interpreter interface can be used at any one time. For that
reason, no attempt at providing mutual exclusion to the cOutputBuffer array is
attempted.
configAPPLICATION_PROVIDES_cOutputBuffer is provided to allow the application
writer to provide their own cOutputBuffer declaration in cases where the
buffer needs to be placed at a fixed address (rather than by the linker). */
#if( configAPPLICATION_PROVIDES_cOutputBuffer == 0 )
static char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];
#else
extern char cOutputBuffer[ configCOMMAND_INT_MAX_OUTPUT_SIZE ];
#endif
/*---------------------------------------------------------- */
BaseType_t FreeRTOS_CLIRegisterCommand( const CLI_Command_Definition_t * const pxCommandToRegister )
{
static CLI_Definition_List_Item_t *pxLastCommandInList = &xRegisteredCommands;
CLI_Definition_List_Item_t *pxNewListItem;
BaseType_t xReturn = pdFAIL;
/* Check the parameter is not NULL. */
configASSERT( pxCommandToRegister );
/* Create a new list item that will reference the command being registered. */
pxNewListItem = ( CLI_Definition_List_Item_t * ) pvPortMalloc( sizeof( CLI_Definition_List_Item_t ) );
configASSERT( pxNewListItem );
if( pxNewListItem != NULL )
{
taskENTER_CRITICAL();
{
/* Reference the command being registered from the newly created
list item. */
pxNewListItem->pxCommandLineDefinition = pxCommandToRegister;
/* The new list item will get added to the end of the list, so
pxNext has nowhere to point. */
pxNewListItem->pxNext = NULL;
/* Add the newly created list item to the end of the already existing
list. */
pxLastCommandInList->pxNext = pxNewListItem;
/* Set the end of list marker to the new list item. */
pxLastCommandInList = pxNewListItem;
}
taskEXIT_CRITICAL();
xReturn = pdPASS;
}
return xReturn;
}
/*---------------------------------------------------------- */
BaseType_t FreeRTOS_CLIProcessCommand( const char * const pcCommandInput, char * pcWriteBuffer, size_t xWriteBufferLen )
{
static const CLI_Definition_List_Item_t *pxCommand = NULL;
BaseType_t xReturn = pdTRUE;
const char *pcRegisteredCommandString;
size_t xCommandStringLength;
/* Note: This function is not re-entrant. It must not be called from more
thank one task. */
if( pxCommand == NULL )
{
/* Search for the command string in the list of registered commands. */
for( pxCommand = &xRegisteredCommands; pxCommand != NULL; pxCommand = pxCommand->pxNext )
{
pcRegisteredCommandString = pxCommand->pxCommandLineDefinition->pcCommand;
xCommandStringLength = strlen( pcRegisteredCommandString );
/* To ensure the string lengths match exactly, so as not to pick up
a sub-string of a longer command, check the byte after the expected
end of the string is either the end of the string or a space before
a parameter. */
if( ( pcCommandInput[ xCommandStringLength ] == ' ' ) || ( pcCommandInput[ xCommandStringLength ] == 0x00 ) )
{
if( strncmp( pcCommandInput, pcRegisteredCommandString, xCommandStringLength ) == 0 )
{
/* The command has been found. Check it has the expected
number of parameters. If cExpectedNumberOfParameters is -1,
then there could be a variable number of parameters and no
check is made. */
if( pxCommand->pxCommandLineDefinition->cExpectedNumberOfParameters >= 0 )
{
if( prvGetNumberOfParameters( pcCommandInput ) != pxCommand->pxCommandLineDefinition->cExpectedNumberOfParameters )
{
xReturn = pdFALSE;
}
}
break;
}
}
}
}
if( ( pxCommand != NULL ) && ( xReturn == pdFALSE ) )
{
/* The command was found, but the number of parameters with the command
was incorrect. */
strncpy( pcWriteBuffer, "Incorrect command parameter(s). Enter \"help\" to view a list of available commands.\r\n\r\n", xWriteBufferLen );
pxCommand = NULL;
}
else if( pxCommand != NULL )
{
/* Call the callback function that is registered to this command. */
xReturn = pxCommand->pxCommandLineDefinition->pxCommandInterpreter( pcWriteBuffer, xWriteBufferLen, pcCommandInput );
/* If xReturn is pdFALSE, then no further strings will be returned
after this one, and pxCommand can be reset to NULL ready to search
for the next entered command. */
if( xReturn == pdFALSE )
{
pxCommand = NULL;
}
}
else
{
/* pxCommand was NULL, the command was not found. */
strncpy( pcWriteBuffer, "Command not recognised. Enter 'help' to view a list of available commands.\r\n\r\n", xWriteBufferLen );
xReturn = pdFALSE;
}
return xReturn;
}
/*---------------------------------------------------------- */
char *FreeRTOS_CLIGetOutputBuffer( void )
{
return cOutputBuffer;
}
/*---------------------------------------------------------- */
const char *FreeRTOS_CLIGetParameter( const char *pcCommandString, UBaseType_t uxWantedParameter, BaseType_t *pxParameterStringLength )
{
UBaseType_t uxParametersFound = 0;
const char *pcReturn = NULL;
*pxParameterStringLength = 0;
while( uxParametersFound < uxWantedParameter )
{
/* Index the character pointer past the current word. If this is the start
of the command string then the first word is the command itself. */
while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) != ' ' ) )
{
pcCommandString++;
}
/* Find the start of the next string. */
while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) == ' ' ) )
{
pcCommandString++;
}
/* Was a string found? */
if( *pcCommandString != 0x00 )
{
/* Is this the start of the required parameter? */
uxParametersFound++;
if( uxParametersFound == uxWantedParameter )
{
/* How long is the parameter? */
pcReturn = pcCommandString;
while( ( ( *pcCommandString ) != 0x00 ) && ( ( *pcCommandString ) != ' ' ) )
{
( *pxParameterStringLength )++;
pcCommandString++;
}
if( *pxParameterStringLength == 0 )
{
pcReturn = NULL;
}
break;
}
}
else
{
break;
}
}
return pcReturn;
}
/*---------------------------------------------------------- */
static BaseType_t prvHelpCommand( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString )
{
static const CLI_Definition_List_Item_t * pxCommand = NULL;
BaseType_t xReturn;
( void ) pcCommandString;
if( pxCommand == NULL )
{
/* Reset the pxCommand pointer back to the start of the list. */
pxCommand = &xRegisteredCommands;
}
/* Return the next command help string, before moving the pointer on to
the next command in the list. */
strncpy( pcWriteBuffer, pxCommand->pxCommandLineDefinition->pcHelpString, xWriteBufferLen );
pxCommand = pxCommand->pxNext;
if( pxCommand == NULL )
{
/* There are no more commands in the list, so there will be no more
strings to return after this one and pdFALSE should be returned. */
xReturn = pdFALSE;
}
else
{
xReturn = pdTRUE;
}
return xReturn;
}
/*---------------------------------------------------------- */
static int8_t prvGetNumberOfParameters( const char *pcCommandString )
{
int8_t cParameters = 0;
BaseType_t xLastCharacterWasSpace = pdFALSE;
/* Count the number of space delimited words in pcCommandString. */
while( *pcCommandString != 0x00 )
{
if( ( *pcCommandString ) == ' ' )
{
if( xLastCharacterWasSpace != pdTRUE )
{
cParameters++;
xLastCharacterWasSpace = pdTRUE;
}
}
else
{
xLastCharacterWasSpace = pdFALSE;
}
pcCommandString++;
}
/* If the command string ended with spaces, then there will have been too
many parameters counted. */
if( xLastCharacterWasSpace == pdTRUE )
{
cParameters--;
}
/* The value returned is one less than the number of space delimited words,
as the first word should be the command itself. */
return cParameters;
}

108
User/component/freertos_cli.h
View File

@ -1,108 +0,0 @@
/*
* FreeRTOS+CLI V1.0.4
* Copyright (C) 2017 Amazon.com, Inc. or its affiliates. All Rights Reserved.
*
* Permission is hereby granted, free of charge, to any person obtaining a copy of
* this software and associated documentation files (the "Software"), to deal in
* the Software without restriction, including without limitation the rights to
* use, copy, modify, merge, publish, distribute, sublicense, and/or sell copies of
* the Software, and to permit persons to whom the Software is furnished to do so,
* subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS
* FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR
* COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER
* IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN
* CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
*
* http://www.FreeRTOS.org
* http://aws.amazon.com/freertos
*
* 1 tab == 4 spaces!
*/
#ifndef COMMAND_INTERPRETER_H
#define COMMAND_INTERPRETER_H
/* This config should be defined in FreeRTOSConfig.h. But due to the limition of CubeMX I put it here. */
#define configCOMMAND_INT_MAX_OUTPUT_SIZE 512
/* The prototype to which callback functions used to process command line
commands must comply. pcWriteBuffer is a buffer into which the output from
executing the command can be written, xWriteBufferLen is the length, in bytes of
the pcWriteBuffer buffer, and pcCommandString is the entire string as input by
the user (from which parameters can be extracted).*/
typedef BaseType_t (*pdCOMMAND_LINE_CALLBACK)( char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString );
/* The structure that defines command line commands. A command line command
should be defined by declaring a const structure of this type. */
typedef struct xCOMMAND_LINE_INPUT
{
const char * const pcCommand; /* The command that causes pxCommandInterpreter to be executed. For example "help". Must be all lower case. */
const char * const pcHelpString; /* String that describes how to use the command. Should start with the command itself, and end with "\r\n". For example "help: Returns a list of all the commands\r\n". */
const pdCOMMAND_LINE_CALLBACK pxCommandInterpreter; /* A pointer to the callback function that will return the output generated by the command. */
int8_t cExpectedNumberOfParameters; /* Commands expect a fixed number of parameters, which may be zero. */
} CLI_Command_Definition_t;
/* For backward compatibility. */
#define xCommandLineInput CLI_Command_Definition_t
/*
* Register the command passed in using the pxCommandToRegister parameter.
* Registering a command adds the command to the list of commands that are
* handled by the command interpreter. Once a command has been registered it
* can be executed from the command line.
*/
BaseType_t FreeRTOS_CLIRegisterCommand( const CLI_Command_Definition_t * const pxCommandToRegister );
/*
* Runs the command interpreter for the command string "pcCommandInput". Any
* output generated by running the command will be placed into pcWriteBuffer.
* xWriteBufferLen must indicate the size, in bytes, of the buffer pointed to
* by pcWriteBuffer.
*
* FreeRTOS_CLIProcessCommand should be called repeatedly until it returns pdFALSE.
*
* pcCmdIntProcessCommand is not reentrant. It must not be called from more
* than one task - or at least - by more than one task at a time.
*/
BaseType_t FreeRTOS_CLIProcessCommand( const char * const pcCommandInput, char * pcWriteBuffer, size_t xWriteBufferLen );
/*---------------------------------------------------------- */
/*
* A buffer into which command outputs can be written is declared in the
* main command interpreter, rather than in the command console implementation,
* to allow application that provide access to the command console via multiple
* interfaces to share a buffer, and therefore save RAM. Note, however, that
* the command interpreter itself is not re-entrant, so only one command
* console interface can be used at any one time. For that reason, no attempt
* is made to provide any mutual exclusion mechanism on the output buffer.
*
* FreeRTOS_CLIGetOutputBuffer() returns the address of the output buffer.
*/
char *FreeRTOS_CLIGetOutputBuffer( void );
/*
* Return a pointer to the xParameterNumber'th word in pcCommandString.
*/
const char *FreeRTOS_CLIGetParameter( const char *pcCommandString, UBaseType_t uxWantedParameter, BaseType_t *pxParameterStringLength );
#endif /* COMMAND_INTERPRETER_H */

107
User/component/limiter.c
View File

@ -1,107 +0,0 @@
/*
限制器
*/
#include "limiter.h"
#include <math.h>
#include <stddef.h>
#define POWER_BUFF_THRESHOLD 20
#define CHASSIS_POWER_CHECK_FREQ 10
#define CHASSIS_POWER_FACTOR_PASS 0.9f
#define CHASSIS_POWER_FACTOR_NO_PASS 1.5f
#define CHASSIS_MOTOR_CIRCUMFERENCE 0.12f
/**
* @brief 限制底盘功率不超过power_limit
*
* @param power_limit 最大功率
* @param motor_out 电机输出值
* @param speed 电机转速
* @param len 电机数量
* @return int8_t 0对应没有错误
*/
int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
float *speed, uint32_t len) {
/* power_limit小于0时不进行限制 */
if (motor_out == NULL || speed == NULL || power_limit < 0) return -1;
float sum_motor_out = 0.0f;
for (uint32_t i = 0; i < len; i++) {
/* 总功率计算 P=F(由转矩电流表示)*V(由转速表示) */
sum_motor_out +=
fabsf(motor_out[i]) * fabsf(speed[i]) * CHASSIS_MOTOR_CIRCUMFERENCE;
}
/* 保持每个电机输出值缩小时比例不变 */
if (sum_motor_out > power_limit) {
for (uint32_t i = 0; i < len; i++) {
motor_out[i] *= power_limit / sum_motor_out;
}
}
return 0;
}
/**
* @brief 电容输入功率计算
*
* @param power_in 底盘当前功率
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 裁判系统输出最大值
*/
float PowerLimit_CapInput(float power_in, float power_limit,
float power_buffer) {
float target_power = 0.0f;
/* 计算下一个检测周期的剩余缓冲能量 */
float heat_buff = power_buffer - (float)(power_in - power_limit) /
(float)CHASSIS_POWER_CHECK_FREQ;
if (heat_buff < POWER_BUFF_THRESHOLD) { /* 功率限制 */
target_power = power_limit * CHASSIS_POWER_FACTOR_PASS;
} else {
target_power = power_limit * CHASSIS_POWER_FACTOR_NO_PASS;
}
return target_power;
}
/**
* @brief 使用缓冲能量计算底盘最大功率
*
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 底盘输出最大值
*/
float PowerLimit_TargetPower(float power_limit, float power_buffer) {
float target_power = 0.0f;
/* 根据剩余缓冲能量计算输出功率 */
target_power = power_limit * (power_buffer - 10.0f) / 20.0f;
if (target_power < 0.0f) target_power = 0.0f;
return target_power;
}
/**
* @brief 射击频率控制
*
* @param heat 当前热量
* @param heat_limit 热量上限
* @param cooling_rate 冷却速率
* @param heat_increase 冷却增加
* @param shoot_freq 经过热量限制后的射击频率
* @return float 射击频率
*/
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big) {
float heat_percent = heat / heat_limit;
float stable_freq = cooling_rate / heat_increase;
if (is_big)
return stable_freq;
else
return (heat_percent > 0.7f) ? stable_freq : 3.0f * stable_freq;
}

63
User/component/limiter.h
View File

@ -1,63 +0,0 @@
/*
限制器
*/
#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 */
/**
* @brief 限制底盘功率不超过power_limit
*
* @param power_limit 最大功率
* @param motor_out 电机输出值
* @param speed 电机转速
* @param len 电机数量
* @return int8_t 0对应没有错误
*/
int8_t PowerLimit_ChassicOutput(float power_limit, float *motor_out,
float *speed, uint32_t len);
/**
* @brief 电容输入功率计算
*
* @param power_in 底盘当前功率
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 裁判系统输出最大值
*/
float PowerLimit_CapInput(float power_in, float power_limit,
float power_buffer);
/**
* @brief 使用缓冲能量计算底盘最大功率
*
* @param power_limit 裁判系统功率限制值
* @param power_buffer 缓冲能量
* @return float 底盘输出最大值
*/
float PowerLimit_TargetPower(float power_limit, float power_buffer);
/**
* @brief 射击频率控制
*
* @param heat 当前热量
* @param heat_limit 热量上限
* @param cooling_rate 冷却速率
* @param heat_increase 冷却增加
* @param shoot_freq 经过热量限制后的射击频率
* @return float 射击频率
*/
float HeatLimit_ShootFreq(float heat, float heat_limit, float cooling_rate,
float heat_increase, bool is_big);

158
User/component/pid.c
View File

@ -1,158 +0,0 @@
/*
Modified from
https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.cpp
参考资料：
https://github.com/PX4/Firmware/issues/12362
https://dev.px4.io/master/en/flight_stack/controller_diagrams.html
https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html#standard_form
https://www.controleng.com/articles/not-all-pid-controllers-are-the-same/
https://en.wikipedia.org/wiki/PID_controller
http://brettbeauregard.com/blog/2011/04/improving-the-beginner%E2%80%99s-pid-derivative-kick/
*/
#include "pid.h"
#define SIGMA 0.000001f
/**
* @brief 初始化PID
*
* @param pid PID结构体
* @param mode PID模式
* @param sample_freq 采样频率
* @param param PID参数
* @return int8_t 0对应没有错误
*/
int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
const KPID_Params_t *param) {
if (pid == NULL) return -1;
if (!isfinite(param->p)) return -1;
if (!isfinite(param->i)) return -1;
if (!isfinite(param->d)) return -1;
if (!isfinite(param->i_limit)) return -1;
if (!isfinite(param->out_limit)) return -1;
pid->param = param;
float dt_min = 1.0f / sample_freq;
if (isfinite(dt_min))
pid->dt_min = dt_min;
else
return -1;
LowPassFilter2p_Init(&(pid->dfilter), sample_freq, pid->param->d_cutoff_freq);
pid->mode = mode;
PID_Reset(pid);
return 0;
}
/**
* @brief PID计算
*
* @param pid PID结构体
* @param sp 设定值
* @param fb 反馈值
* @param fb_dot 反馈值微分
* @param dt 间隔时间
* @return float 计算的输出
*/
float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt) {
if (!isfinite(sp) || !isfinite(fb) || !isfinite(fb_dot) || !isfinite(dt)) {
return pid->last.out;
}
/* 计算误差值 */
const float err = CircleError(sp, fb, pid->param->range);
/* 计算P项 */
const float k_err = err * pid->param->k;
/* 计算D项 */
const float k_fb = pid->param->k * fb;
const float filtered_k_fb = LowPassFilter2p_Apply(&(pid->dfilter), k_fb);
float d;
switch (pid->mode) {
case KPID_MODE_CALC_D:
/* 通过fb计算D，避免了由于sp变化导致err突变的问题 */
/* 当sp不变时，err的微分等于负的fb的微分 */
d = (filtered_k_fb - pid->last.k_fb) / fmaxf(dt, pid->dt_min);
break;
case KPID_MODE_SET_D:
d = fb_dot;
break;
case KPID_MODE_NO_D:
d = 0.0f;
break;
}
pid->last.err = err;
pid->last.k_fb = filtered_k_fb;
if (!isfinite(d)) d = 0.0f;
/* 计算PD输出 */
float output = (k_err * pid->param->p) - (d * pid->param->d);
/* 计算I项 */
const float i = pid->i + (k_err * dt);
const float i_out = i * pid->param->i;
if (pid->param->i > SIGMA) {
/* 检查是否饱和 */
if (isfinite(i)) {
if ((fabsf(output + i_out) <= pid->param->out_limit) &&
(fabsf(i) <= pid->param->i_limit)) {
/* 未饱和，使用新积分 */
pid->i = i;
}
}
}
/* 计算PID输出 */
output += i_out;
/* 限制输出 */
if (isfinite(output)) {
if (pid->param->out_limit > SIGMA) {
output = AbsClip(output, pid->param->out_limit);
}
pid->last.out = output;
}
return pid->last.out;
}
/**
* @brief 重置微分项
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_ResetIntegral(KPID_t *pid) {
if (pid == NULL) return -1;
pid->i = 0.0f;
return 0;
}
/**
* @brief 重置PID
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_Reset(KPID_t *pid) {
if (pid == NULL) return -1;
pid->i = 0.0f;
pid->last.err = 0.0f;
pid->last.k_fb = 0.0f;
pid->last.out = 0.0f;
LowPassFilter2p_Reset(&(pid->dfilter), 0.0f);
return 0;
}

107
User/component/pid.h
View File

@ -1,107 +0,0 @@
/*
Modified from
https://github.com/PX4/Firmware/blob/master/src/lib/pid/pid.h
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include "filter.h"
#include "user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* PID模式 */
typedef enum {
KPID_MODE_NO_D = 0, /* 不使用微分项，PI控制器 */
KPID_MODE_CALC_D, /* 根据反馈的值计算离散微分，忽略PID_Calc中的fb_dot */
KPID_MODE_SET_D /* 直接提供微分值，PID_Calc中的fb_dot将被使用，(Gyros) */
} KPID_Mode_t;
/* PID参数 */
typedef struct {
float k; /* 控制器增益，设置为1用于并行模式 */
float p; /* 比例项增益，设置为1用于标准形式 */
float i; /* 积分项增益 */
float d; /* 微分项增益 */
float i_limit; /* 积分项上限 */
float out_limit; /* 输出绝对值限制 */
float d_cutoff_freq; /* D项低通截止频率 */
float range; /* 计算循环误差时使用，大于0时启用 */
} KPID_Params_t;
/* PID主结构体 */
typedef struct {
KPID_Mode_t mode;
const KPID_Params_t *param;
float dt_min; /* 最小PID_Calc调用间隔 */
float i; /* 积分 */
struct {
float err; /* 上次误差 */
float k_fb; /* 上次反馈值 */
float out; /* 上次输出 */
} last;
LowPassFilter2p_t dfilter; /* D项低通滤波器 */
} KPID_t;
/**
* @brief 初始化PID
*
* @param pid PID结构体
* @param mode PID模式
* @param sample_freq 采样频率
* @param param PID参数
* @return int8_t 0对应没有错误
*/
int8_t PID_Init(KPID_t *pid, KPID_Mode_t mode, float sample_freq,
const KPID_Params_t *param);
/**
* @brief PID计算
*
* @param pid PID结构体
* @param sp 设定值
* @param fb 反馈值
* @param fb_dot 反馈值微分
* @param dt 间隔时间
* @return float 计算的输出
*/
float PID_Calc(KPID_t *pid, float sp, float fb, float fb_dot, float dt);
/**
* @brief 重置微分项
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_ResetIntegral(KPID_t *pid);
/**
* @brief 重置PID
*
* @param pid PID结构体
* @return int8_t 0对应没有错误
*/
int8_t PID_Reset(KPID_t *pid);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

134
User/component/user_math.c
View File

@ -1,134 +0,0 @@
/*
自定义的数学运算。
*/
#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
View File

@ -1,179 +0,0 @@
/*
自定义的数学运算。
*/
#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
View File

@ -1,381 +0,0 @@
/*
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
View File

@ -1,81 +0,0 @@
#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

171
User/device/buzzer.c
View File

@ -1,171 +0,0 @@
#include "device/buzzer.h"
#include "bsp/time.h"
#include <math.h>
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
#define MUSIC_DEFAULT_VOLUME 0.5f
#define MUSIC_A4_FREQ 440.0f // A4音符频率
/* USER MUSIC MENU BEGIN */
// RM音乐
const Tone_t RM[] = {
{NOTE_B, 5, 200},
{NOTE_G, 4, 200},
{NOTE_B, 5, 400},
{NOTE_G, 4, 200},
{NOTE_B, 5, 400},
{NOTE_G, 4, 200},
{NOTE_D, 5, 400},
{NOTE_G, 4, 200},
{NOTE_C, 5, 200},
{NOTE_C, 5, 200},
{NOTE_G, 4, 200},
{NOTE_B, 5, 200},
{NOTE_C, 5, 200}
};
// Nokia 经典铃声音符
const Tone_t NOKIA[] = {
{NOTE_E, 5, 125}, {NOTE_D, 5, 125}, {NOTE_FS, 4, 250}, {NOTE_GS, 4, 250},
{NOTE_CS, 5, 125}, {NOTE_B, 4, 125}, {NOTE_D, 4, 250}, {NOTE_E, 4, 250},
{NOTE_B, 4, 125}, {NOTE_A, 4, 125}, {NOTE_CS, 4, 250}, {NOTE_E, 4, 250},
{NOTE_A, 4, 500}
};
/* USER MUSIC MENU END */
static void BUZZER_Update(BUZZER_t *buzzer){
buzzer->header.online = true;
buzzer->header.last_online_time = BSP_TIME_Get_ms();
}
// 根据音符和八度计算频率的辅助函数
static float BUZZER_CalcFreq(NOTE_t note, uint8_t octave) {
if (note == NOTE_REST) {
return 0.0f; // 休止符返回0频率
}
// 将音符和八度转换为MIDI音符编号
int midi_num = (int)note + (int)((octave + 1) * 12);
// 使用A4 (440Hz) 作为参考，计算频率
// 公式: freq = 440 * 2^((midi_num - 69)/12)
float freq = 440.0f * powf(2.0f, ((float)midi_num - 69.0f) / 12.0f);
return freq;
}
// 播放单个音符
static int8_t BUZZER_PlayTone(BUZZER_t *buzzer, NOTE_t note, uint8_t octave, uint16_t duration_ms) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
float freq = BUZZER_CalcFreq(note, octave);
if (freq > 0.0f) {
// 播放音符
if (BUZZER_Set(buzzer, freq, MUSIC_DEFAULT_VOLUME) != DEVICE_OK)
return DEVICE_ERR;
if (BUZZER_Start(buzzer) != DEVICE_OK)
return DEVICE_ERR;
} else {
// 休止符，停止播放
BUZZER_Stop(buzzer);
}
// 等待指定时间
BSP_TIME_Delay_ms(duration_ms);
// 停止当前音符，为下一个音符做准备
BUZZER_Stop(buzzer);
BSP_TIME_Delay_ms(20); // 短暂间隔
return DEVICE_OK;
}
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel) {
if (buzzer == NULL) return DEVICE_ERR;
buzzer->channel = channel;
buzzer->header.online = true;
BUZZER_Stop(buzzer);
return DEVICE_OK ;
}
int8_t BUZZER_Start(BUZZER_t *buzzer) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
BUZZER_Update(buzzer);
return (BSP_PWM_Start(buzzer->channel) == BSP_OK) ?
DEVICE_OK : DEVICE_ERR;
}
int8_t BUZZER_Stop(BUZZER_t *buzzer) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
BUZZER_Update(buzzer);
return (BSP_PWM_Stop(buzzer->channel) == BSP_OK) ?
DEVICE_OK : DEVICE_ERR;
}
int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
int result = DEVICE_OK ;
BUZZER_Update(buzzer);
if (BSP_PWM_SetFreq(buzzer->channel, freq) != BSP_OK)
result = DEVICE_ERR;
if (BSP_PWM_SetComp(buzzer->channel, duty_cycle) != BSP_OK)
result = DEVICE_ERR;
return result;
}
int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music) {
if (buzzer == NULL || !buzzer->header.online)
return DEVICE_ERR;
const Tone_t *melody = NULL;
size_t melody_length = 0;
// 根据音乐类型选择对应的音符数组
switch (music) {
case MUSIC_RM:
melody = RM;
melody_length = sizeof(RM) / sizeof(Tone_t);
break;
case MUSIC_NOKIA:
melody = NOKIA;
melody_length = sizeof(NOKIA) / sizeof(Tone_t);
break;
default:
return DEVICE_ERR;
}
// 播放整首音乐
for (size_t i = 0; i < melody_length; i++) {
if (BUZZER_PlayTone(buzzer, melody[i].note, melody[i].octave, melody[i].duration_ms) != DEVICE_OK) {
BUZZER_Stop(buzzer); // 出错时停止播放
return DEVICE_ERR;
}
}
// 音乐播放完成后停止
BUZZER_Stop(buzzer);
return DEVICE_OK;
}
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */

138
User/device/buzzer.h
View File

@ -1,138 +0,0 @@
/**
* @file buzzer.h
* @brief 蜂鸣器设备驱动头文件
* @details 提供蜂鸣器音频播放功能，支持单音符播放和预设音乐播放
* @author Generated by STM32CubeMX
* @date 2025年10月23日
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "bsp/pwm.h" // PWM底层硬件抽象层
#include "device.h" // 设备通用头文件
#include <stddef.h> // 标准定义
#include <stdint.h> // 标准整型定义
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Exported constants ------------------------------------------------------- */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Exported types ----------------------------------------------------------- */
/**
* @brief 音符枚举类型
* @details 定义标准十二平均律音符，用于音乐播放
*/
typedef enum {
NOTE_C = 0, ///< Do音符
NOTE_CS = 1, ///< Do#音符 (升Do)
NOTE_D = 2, ///< Re音符
NOTE_DS = 3, ///< Re#音符 (升Re)
NOTE_E = 4, ///< Mi音符
NOTE_F = 5, ///< Fa音符
NOTE_FS = 6, ///< Fa#音符 (升Fa)
NOTE_G = 7, ///< Sol音符
NOTE_GS = 8, ///< Sol#音符 (升Sol)
NOTE_A = 9, ///< La音符
NOTE_AS = 10, ///< La#音符 (升La)
NOTE_B = 11, ///< Si音符
NOTE_REST = 255 ///< 休止符 (无声音)
} NOTE_t;
/**
* @brief 音调结构体
* @details 定义一个完整的音调信息，包括音符、八度和持续时间
*/
typedef struct {
NOTE_t note; ///< 音符名称 (使用NOTE_t枚举)
uint8_t octave; ///< 八度 (0-8，通常使用3-7)
uint16_t duration_ms; ///< 持续时间，单位毫秒
} Tone_t;
/**
* @brief 预设音乐枚举类型
* @details 定义可播放的预设音乐类型
*/
typedef enum {
/* USER MUSIC MENU BEGIN */
MUSIC_RM, ///< RM战队音乐
MUSIC_NOKIA, ///< 诺基亚经典铃声
/* USER MUSIC MENU END */
} MUSIC_t;
/**
* @brief 蜂鸣器设备结构体
* @details 蜂鸣器设备的完整描述，包含设备头信息和PWM通道
*/
typedef struct {
DEVICE_Header_t header; ///< 设备通用头信息 (在线状态、时间戳等)
BSP_PWM_Channel_t channel; ///< PWM输出通道
} BUZZER_t;
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化蜂鸣器设备
* @param buzzer 蜂鸣器设备结构体指针
* @param channel PWM输出通道
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
* @note 初始化后蜂鸣器处于停止状态
*/
int8_t BUZZER_Init(BUZZER_t *buzzer, BSP_PWM_Channel_t channel);
/**
* @brief 启动蜂鸣器播放
* @param buzzer 蜂鸣器设备结构体指针
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
* @note 需要先调用BUZZER_Set设置频率和占空比
*/
int8_t BUZZER_Start(BUZZER_t *buzzer);
/**
* @brief 停止蜂鸣器播放
* @param buzzer 蜂鸣器设备结构体指针
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
*/
int8_t BUZZER_Stop(BUZZER_t *buzzer);
/**
* @brief 设置蜂鸣器频率和占空比
* @param buzzer 蜂鸣器设备结构体指针
* @param freq 频率 (Hz)，通常范围20Hz-20kHz
* @param duty_cycle 占空比 (0.0-1.0)，影响音量大小
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
* @note 设置后需要调用BUZZER_Start才能听到声音
*/
int8_t BUZZER_Set(BUZZER_t *buzzer, float freq, float duty_cycle);
/**
* @brief 播放预设音乐
* @param buzzer 蜂鸣器设备结构体指针
* @param music 音乐类型 (使用MUSIC_t枚举)
* @return int8_t 返回值：DEVICE_OK(0) 成功，DEVICE_ERR(-1) 失败
* @note 这是一个阻塞函数，会播放完整首音乐后返回
*/
int8_t BUZZER_PlayMusic(BUZZER_t *buzzer, MUSIC_t music);
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
#ifdef __cplusplus
}
#endif

51
User/device/device.h
View File

@ -1,51 +0,0 @@
#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

31
User/device/device_config.yaml
View File

@ -1,31 +0,0 @@
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
buzzer:
bsp_config:
BSP_PWM_BUZZER: BSP_PWM_TIM12_CH2
enabled: true
dr16:
bsp_config:
BSP_UART_DR16: BSP_UART_DR16
enabled: true
motor:
bsp_config: {}
enabled: true
motor_dm:
bsp_config: {}
enabled: true
motor_lk:
bsp_config: {}
enabled: true
motor_lz:
bsp_config: {}
enabled: true
motor_rm:
bsp_config: {}
enabled: true

169
User/device/dr16.c
View File

@ -1,169 +0,0 @@
/*
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
View File

@ -1,117 +0,0 @@
#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
View File

@ -1,52 +0,0 @@
/*
电机通用函数
*/
/* 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
View File

@ -1,68 +0,0 @@
#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

496
User/device/motor_dm.c
View File

@ -1,496 +0,0 @@
#define MOTOR_DM_FLOAT_TO_INT_SIGNED(x, x_min, x_max, bits) \
((int32_t)roundf(((x) / ((x_max) - (x_min))) * (1 << (bits)) + (1 << ((bits) - 1))))
#define MOTOR_DM_INT_TO_FLOAT_SIGNED(x_int, x_min, x_max, bits) \
(((float)((int32_t)(x_int) - (1 << ((bits) - 1))) * ((x_max) - (x_min)) / (float)(1 << (bits))))
/* Includes ----------------------------------------------------------------- */
#include "device/motor_dm.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
#include "string.h"
#include "math.h"
/* Private constants -------------------------------------------------------- */
/* DM电机数据映射范围 */
#define DM_P_MIN (-12.56637f)
#define DM_P_MAX (12.56637f)
#define DM_V_MIN (-30.0f)
#define DM_V_MAX (30.0f)
#define DM_T_MIN (-12.0f)
#define DM_T_MAX (12.0f)
#define DM_KP_MIN (0.0f)
#define DM_KP_MAX (500.0f)
#define DM_KD_MIN (0.0f)
#define DM_KD_MAX (5.0f)
/* CAN ID偏移量 */
#define DM_CAN_ID_OFFSET_POS_VEL 0x100
#define DM_CAN_ID_OFFSET_VEL 0x200
/* Private macro ------------------------------------------------------------ */
#define FLOAT_TO_UINT(x, x_min, x_max, bits) \
(uint32_t)((x - x_min) * ((1 << bits) - 1) / (x_max - x_min))
#define UINT_TO_FLOAT(x_int, x_min, x_max, bits) \
((float)(x_int) * (x_max - x_min) / ((1 << bits) - 1) + x_min)
/* Private variables -------------------------------------------------------- */
static MOTOR_DM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
static int float_to_uint(float x_float, float x_min, float x_max, int bits)
{
/* Converts a float to an unsigned int, given range and number of bits */
float span = x_max - x_min;
float offset = x_min;
return (int) ((x_float-offset)*((float)((1<<bits)-1))/span);
}
static float uint_to_float(int x_int, float x_min, float x_max, int bits)
{
/* converts unsigned int to float, given range and number of bits */
float span = x_max - x_min;
float offset = x_min;
return ((float)x_int)*span/((float)((1<<bits)-1)) + offset;
}
/* Private function prototypes ---------------------------------------------- */
static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data);
static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output);
static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel);
static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel);
static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can);
/* Private functions -------------------------------------------------------- */
/**
* @brief 解析DM电机反馈帧
* @param motor 电机实例
* @param data CAN数据
* @return 解析结果
*/
static int8_t MOTOR_DM_ParseFeedbackFrame(MOTOR_DM_t *motor, const uint8_t *data) {
if (motor == NULL || data == NULL) {
return DEVICE_ERR_NULL;
}
uint16_t p_int=(data[1]<<8)|data[2];
motor->motor.feedback.rotor_abs_angle = uint_to_float(p_int, DM_P_MIN, DM_P_MAX, 16); // (-12.5,12.5)
uint16_t v_int=(data[3]<<4)|(data[4]>>4);
motor->motor.feedback.rotor_speed = uint_to_float(v_int, DM_V_MIN, DM_V_MAX, 12); // (-30.0,30.0)
uint16_t t_int=((data[4]&0xF)<<8)|data[5];
motor->motor.feedback.torque_current = uint_to_float(t_int, DM_T_MIN, DM_T_MAX, 12); // (-12.0,12.0)
motor->motor.feedback.temp = (float)(data[6]);
while (motor->motor.feedback.rotor_abs_angle < 0) {
motor->motor.feedback.rotor_abs_angle += M_2PI;
}
while (motor->motor.feedback.rotor_abs_angle >= M_2PI) {
motor->motor.feedback.rotor_abs_angle -= M_2PI;
}
if (motor->param.reverse) {
motor->motor.feedback.rotor_abs_angle = M_2PI - motor->motor.feedback.rotor_abs_angle;
motor->motor.feedback.rotor_speed = -motor->motor.feedback.rotor_speed;
motor->motor.feedback.torque_current = -motor->motor.feedback.torque_current;
}
return DEVICE_OK;
}
/**
* @brief 发送MIT模式控制命令
* @param motor 电机实例
* @param output MIT控制参数
* @return 发送结果
*/
static int8_t MOTOR_DM_SendMITCmd(MOTOR_DM_t *motor, MOTOR_MIT_Output_t *output) {
if (motor == NULL || output == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[8];
uint16_t pos_tmp,vel_tmp,kp_tmp,kd_tmp,tor_tmp;
uint16_t id = motor->param.can_id;
pos_tmp = float_to_uint(output->angle, DM_P_MIN , DM_P_MAX, 16);
vel_tmp = float_to_uint(output->velocity, DM_V_MIN , DM_V_MAX, 12);
kp_tmp = float_to_uint(output->kp, DM_KP_MIN, DM_KP_MAX, 12);
kd_tmp = float_to_uint(output->kd, DM_KD_MIN, DM_KD_MAX, 12);
tor_tmp = float_to_uint(output->torque, DM_T_MIN , DM_T_MAX, 12);
/* 打包数据 */
data[0] = (pos_tmp >> 8);
data[1] = pos_tmp;
data[2] = (vel_tmp >> 4);
data[3] = ((vel_tmp&0xF)<<4)|(kp_tmp>>8);
data[4] = kp_tmp;
data[5] = (kd_tmp >> 4);
data[6] = ((kd_tmp&0xF)<<4)|(tor_tmp>>8);
data[7] = tor_tmp;
/* 发送CAN消息 */
BSP_CAN_StdDataFrame_t frame;
frame.id = motor->param.can_id;
frame.dlc = 8;
memcpy(frame.data, data, 8);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 发送位置速度模式控制命令
* @param motor 电机实例
* @param pos 目标位置
* @param vel 目标速度
* @return 发送结果
*/
static int8_t MOTOR_DM_SendPosVelCmd(MOTOR_DM_t *motor, float pos, float vel) {
if (motor == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[8] = {0};
/* 直接发送浮点数数据 */
memcpy(&data[0], &pos, 4); // 位置，低位在前
memcpy(&data[4], &vel, 4); // 速度，低位在前
/* 发送CAN消息，ID为原ID+0x100 */
uint32_t can_id = DM_CAN_ID_OFFSET_POS_VEL + motor->param.can_id;
BSP_CAN_StdDataFrame_t frame;
frame.id = can_id;
frame.dlc = 8;
memcpy(frame.data, data, 8);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 发送速度模式控制命令
* @param motor 电机实例
* @param vel 目标速度
* @return 发送结果
*/
static int8_t MOTOR_DM_SendVelCmd(MOTOR_DM_t *motor, float vel) {
if (motor == NULL) {
return DEVICE_ERR_NULL;
}
uint8_t data[4] = {0};
/* 直接发送浮点数数据 */
memcpy(&data[0], &vel, 4); // 速度，低位在前
/* 发送CAN消息，ID为原ID+0x200 */
uint32_t can_id = DM_CAN_ID_OFFSET_VEL + motor->param.can_id;
BSP_CAN_StdDataFrame_t frame;
frame.id = can_id;
frame.dlc = 4;
memcpy(frame.data, data, 4);
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 获取指定CAN总线的管理器
* @param can CAN总线
* @return CAN管理器指针
*/
static MOTOR_DM_CANManager_t* MOTOR_DM_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) {
return NULL;
}
return can_managers[can];
}
/**
* @brief 创建CAN管理器
* @param can CAN总线
* @return 创建结果
*/
static int8_t MOTOR_DM_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_DM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_DM_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_DM_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 注册一个DM电机
* @param param 电机参数
* @return 注册结果
*/
int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
/* 创建CAN管理器 */
if (MOTOR_DM_CreateCANManager(param->can) != DEVICE_OK) {
return DEVICE_ERR;
}
/* 获取CAN管理器 */
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return DEVICE_ERR;
}
/* 检查是否已注册 */
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
return DEVICE_ERR_INITED;
}
}
/* 检查是否已达到最大数量 */
if (manager->motor_count >= MOTOR_DM_MAX_MOTORS) {
return DEVICE_ERR;
}
/* 分配内存 */
MOTOR_DM_t *motor = (MOTOR_DM_t *)BSP_Malloc(sizeof(MOTOR_DM_t));
if (motor == NULL) {
return DEVICE_ERR;
}
/* 初始化电机 */
memset(motor, 0, sizeof(MOTOR_DM_t));
memcpy(&motor->param, param, sizeof(MOTOR_DM_Param_t));
motor->motor.header.online = false;
motor->motor.reverse = param->reverse;
/* 注册CAN接收ID - DM电机使用Master ID接收反馈 */
uint16_t feedback_id = param->master_id;
if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
BSP_Free(motor);
return DEVICE_ERR;
}
/* 添加到管理器 */
manager->motors[manager->motor_count] = motor;
manager->motor_count++;
return DEVICE_OK;
}
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return 更新结果
*/
int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return DEVICE_ERR_NO_DEV;
}
/* 查找电机 */
MOTOR_DM_t *motor = NULL;
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.master_id == param->master_id) {
motor = manager->motors[i];
break;
}
}
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
/* 主动接收CAN消息 */
uint16_t feedback_id = param->master_id;
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 100000) { // 100ms超时，单位微秒
motor->motor.header.online = false;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
MOTOR_DM_ParseFeedbackFrame(motor, rx_msg.data);
return DEVICE_OK;
}
/**
* @brief 更新所有电机数据
* @return 更新结果
*/
int8_t MOTOR_DM_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_DM_t *motor = manager->motors[i];
if (motor != NULL) {
if (MOTOR_DM_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
/**
* @brief MIT模式控制
* @param param 电机参数
* @param output MIT控制参数
* @return 控制结果
*/
int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output) {
if (param == NULL || output == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendMITCmd(motor, output);
}
/**
* @brief 位置速度模式控制
* @param param 电机参数
* @param target_pos 目标位置
* @param target_vel 目标速度
* @return 控制结果
*/
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendPosVelCmd(motor, target_pos, target_vel);
}
/**
* @brief 速度模式控制
* @param param 电机参数
* @param target_vel 目标速度
* @return 控制结果
*/
int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
return MOTOR_DM_SendVelCmd(motor, target_vel);
}
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return 电机实例指针
*/
MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return NULL;
}
MOTOR_DM_CANManager_t *manager = MOTOR_DM_GetCANManager(param->can);
if (manager == NULL) {
return NULL;
}
/* 查找对应的电机 */
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_DM_t *motor = manager->motors[i];
if (motor && motor->param.can == param->can &&
motor->param.master_id == param->master_id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param){
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
BSP_CAN_StdDataFrame_t frame;
frame.id = motor->param.can_id;
frame.dlc = 8;
frame.data[0] = 0XFF;
frame.data[1] = 0xFF;
frame.data[2] = 0xFF;
frame.data[3] = 0xFF;
frame.data[4] = 0xFF;
frame.data[5] = 0xFF;
frame.data[6] = 0xFF;
frame.data[7] = 0xFC;
return BSP_CAN_TransmitStdDataFrame(motor->param.can, &frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 使电机松弛（设置输出为0）
* @param param 电机参数
* @return 操作结果
*/
int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_MIT_Output_t output = {0};
return MOTOR_DM_MITCtrl(param, &output);
}
/**
* @brief 使电机离线（设置在线状态为false）
* @param param 电机参数
* @return 操作结果
*/
int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param) {
if (param == NULL) {
return DEVICE_ERR_NULL;
}
MOTOR_DM_t *motor = MOTOR_DM_GetMotor(param);
if (motor == NULL) {
return DEVICE_ERR_NO_DEV;
}
motor->motor.header.online = false;
return DEVICE_OK;
}

98
User/device/motor_dm.h
View File

@ -1,98 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_DM_MAX_MOTORS 32
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_DM_J4310,
} MOTOR_DM_Module_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t master_id; /* 主站ID，用于发送控制命令 */
uint16_t can_id; /* 反馈ID，用于接收电机反馈 */
MOTOR_DM_Module_t module;
bool reverse;
} MOTOR_DM_Param_t;
/*电机实例*/
typedef struct{
MOTOR_DM_Param_t param;
MOTOR_t motor;
} MOTOR_DM_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_DM_t *motors[MOTOR_DM_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_DM_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个LK电机
* @param param 电机参数
* @return
*/
int8_t MOTOR_DM_Register(MOTOR_DM_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t MOTOR_DM_Update(MOTOR_DM_Param_t *param);
/**
* @brief 更新所有电机数据
* @return
*/
int8_t MOTOR_DM_UpdateAll(void);
int8_t MOTOR_DM_MITCtrl(MOTOR_DM_Param_t *param, MOTOR_MIT_Output_t *output);
int8_t MOTOR_DM_PosVelCtrl(MOTOR_DM_Param_t *param, float target_pos, float target_vel);
int8_t MOTOR_DM_VelCtrl(MOTOR_DM_Param_t *param, float target_vel);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
MOTOR_DM_t* MOTOR_DM_GetMotor(MOTOR_DM_Param_t *param);
int8_t MOTOR_DM_Enable(MOTOR_DM_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t MOTOR_DM_Relax(MOTOR_DM_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t MOTOR_DM_Offine(MOTOR_DM_Param_t *param);
#ifdef __cplusplus
}
#endif

329
User/device/motor_lk.c
View File

@ -1,329 +0,0 @@
/*
LK电机驱动
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_lk.h"
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
#define LK_CTRL_ID_BASE (0x140)
#define LK_FB_ID_BASE (0x240)
// LK电机命令字节定义
#define LK_CMD_FEEDBACK (0x9C) // 反馈命令字节
#define LK_CMD_MOTOR_OFF (0x80) // 电机关闭命令
#define LK_CMD_MOTOR_ON (0x88) // 电机运行命令
#define LK_CMD_TORQUE_CTRL (0xA1) // 转矩闭环控制命令
// LK电机参数定义
#define LK_CURR_LSB_MF (33.0f / 4096.0f) // MF电机转矩电流分辨率 A/LSB
#define LK_CURR_LSB_MG (66.0f / 4096.0f) // MG电机转矩电流分辨率 A/LSB
#define LK_POWER_RANGE_MS (1000) // MS电机功率范围 ±1000
#define LK_TORQUE_RANGE (2048) // 转矩控制值范围 ±2048
#define LK_TORQUE_CURRENT_MF (16.5f) // MF电机最大转矩电流 A
#define LK_TORQUE_CURRENT_MG (33.0f) // MG电机最大转矩电流 A
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
// 编码器分辨率定义
#define LK_ENC_14BIT_MAX (16383) // 14位编码器最大值
#define LK_ENC_15BIT_MAX (32767) // 15位编码器最大值
#define LK_ENC_16BIT_MAX (65535) // 16位编码器最大值
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
static MOTOR_LK_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
/* Private functions -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
static float MOTOR_LK_GetCurrentLSB(MOTOR_LK_Module_t module) {
switch (module) {
case MOTOR_LK_MF9025:
case MOTOR_LK_MF9035:
return LK_CURR_LSB_MF;
default:
return LK_CURR_LSB_MG; // 默认使用MG的分辨率
}
}
static uint16_t MOTOR_LK_GetEncoderMax(MOTOR_LK_Module_t module) {
// 根据电机型号返回编码器最大值，这里假设都使用16位编码器
// 实际使用时需要根据具体电机型号配置
return LK_ENC_16BIT_MAX;
}
static MOTOR_LK_CANManager_t* MOTOR_LK_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
static int8_t MOTOR_LK_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_LK_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LK_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_LK_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
static void MOTOR_LK_Decode(MOTOR_LK_t *motor, BSP_CAN_Message_t *msg) {
// 检查命令字节是否为反馈命令
if (msg->data[0] != LK_CMD_FEEDBACK) {
// 如果不是标准反馈命令，可能是其他格式的数据
// 临时跳过命令字节检查，直接解析数据
// return;
}
// 解析温度 (DATA[1])
motor->motor.feedback.temp = (int8_t)msg->data[1];
// 解析转矩电流值或功率值 (DATA[2], DATA[3])
int16_t raw_current_or_power = (int16_t)((msg->data[3] << 8) | msg->data[2]);
// 根据电机类型解析电流或功率
switch (motor->param.module) {
case MOTOR_LK_MF9025:
case MOTOR_LK_MF9035:
motor->motor.feedback.torque_current = raw_current_or_power * MOTOR_LK_GetCurrentLSB(motor->param.module);
break;
default:
motor->motor.feedback.torque_current = (float)raw_current_or_power;
break;
}
// 解析转速 (DATA[4], DATA[5]) - 单位：1dps/LSB
int16_t raw_speed = (int16_t)((msg->data[5] << 8) | msg->data[4]);
motor->motor.feedback.rotor_speed = motor->param.reverse ? -raw_speed : raw_speed;
// 解析编码器值 (DATA[6], DATA[7])
uint16_t raw_encoder = (uint16_t)((msg->data[7] << 8) | msg->data[6]);
uint16_t encoder_max = MOTOR_LK_GetEncoderMax(motor->param.module);
// 将编码器值转换为弧度 (0 ~ 2π)
float angle = (float)raw_encoder / (float)encoder_max * M_2PI;
motor->motor.feedback.rotor_abs_angle = motor->param.reverse ? (M_2PI - angle) : angle;
}
/* Exported functions ------------------------------------------------------- */
int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_LK_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
return DEVICE_ERR_INITED;
}
}
// 检查数量
if (manager->motor_count >= MOTOR_LK_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
MOTOR_LK_t *new_motor = (MOTOR_LK_t*)BSP_Malloc(sizeof(MOTOR_LK_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(MOTOR_LK_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse;
// 对于某些LK电机，反馈数据可能通过命令ID发送
// 根据实际测试，使用命令ID接收反馈数据
uint16_t feedback_id = param->id; // 使用命令ID作为反馈ID
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, feedback_id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LK_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
// 对于某些LK电机，反馈数据通过命令ID发送
uint16_t feedback_id = param->id;
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, feedback_id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 1000) {
motor->motor.header.online = false;
return DEVICE_ERR_NO_DEV;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
MOTOR_LK_Decode(motor, &rx_msg);
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
int8_t MOTOR_LK_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LK_t *motor = manager->motors[i];
if (motor != NULL) {
if (MOTOR_LK_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
// 限制输出值范围
if (value > 1.0f) value = 1.0f;
if (value < -1.0f) value = -1.0f;
MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 根据反转参数调整输出
float output = param->reverse ? -value : value;
// 转矩闭环控制命令 - 将输出值转换为转矩控制值
int16_t torque_control = (int16_t)(output * (float)LK_TORQUE_RANGE);
// 构建CAN帧
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
tx_frame.data[0] = LK_CMD_TORQUE_CTRL;
tx_frame.data[1] = 0x00;
tx_frame.data[2] = 0x00;
tx_frame.data[3] = 0x00;
tx_frame.data[4] = (uint8_t)(torque_control & 0xFF);
tx_frame.data[5] = (uint8_t)((torque_control >> 8) & 0xFF);
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param) {
// 对于LK电机，每次设置输出时就直接发送控制命令
// 这个函数可以用于发送其他控制命令，如电机开启/关闭
return DEVICE_OK;
}
int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 电机运行命令
tx_frame.data[0] = LK_CMD_MOTOR_ON; // 命令字节
tx_frame.data[1] = 0x00;
tx_frame.data[2] = 0x00;
tx_frame.data[3] = 0x00;
tx_frame.data[4] = 0x00;
tx_frame.data[5] = 0x00;
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
BSP_CAN_StdDataFrame_t tx_frame;
tx_frame.id = param->id;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
// 电机关闭命令
tx_frame.data[0] = LK_CMD_MOTOR_OFF; // 命令字节
tx_frame.data[1] = 0x00;
tx_frame.data[2] = 0x00;
tx_frame.data[3] = 0x00;
tx_frame.data[4] = 0x00;
tx_frame.data[5] = 0x00;
tx_frame.data[6] = 0x00;
tx_frame.data[7] = 0x00;
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param) {
if (param == NULL) return NULL;
MOTOR_LK_CANManager_t *manager = MOTOR_LK_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LK_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param) {
return MOTOR_LK_SetOutput(param, 0.0f);
}
int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param) {
MOTOR_LK_t *motor = MOTOR_LK_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}

119
User/device/motor_lk.h
View File

@ -1,119 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_LK_MAX_MOTORS 32
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_LK_MF9025,
MOTOR_LK_MF9035,
} MOTOR_LK_Module_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t id;
MOTOR_LK_Module_t module;
bool reverse;
} MOTOR_LK_Param_t;
/*电机实例*/
typedef struct{
MOTOR_LK_Param_t param;
MOTOR_t motor;
} MOTOR_LK_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_LK_t *motors[MOTOR_LK_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_LK_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个LK电机
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_Register(MOTOR_LK_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_Update(MOTOR_LK_Param_t *param);
/**
* @brief 设置一个电机的输出
* @param param 电机参数
* @param value 输出值，范围[-1.0, 1.0]
* @return
*/
int8_t MOTOR_LK_SetOutput(MOTOR_LK_Param_t *param, float value);
/**
* @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_Ctrl(MOTOR_LK_Param_t *param);
/**
* @brief 发送电机开启命令
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_MotorOn(MOTOR_LK_Param_t *param);
/**
* @brief 发送电机关闭命令
* @param param 电机参数
* @return
*/
int8_t MOTOR_LK_MotorOff(MOTOR_LK_Param_t *param);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
MOTOR_LK_t* MOTOR_LK_GetMotor(MOTOR_LK_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t MOTOR_LK_Relax(MOTOR_LK_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t MOTOR_LK_Offine(MOTOR_LK_Param_t *param);
/**
* @brief
* @param
* @return
*/
int8_t MOTOR_LK_UpdateAll(void);
#ifdef __cplusplus
}
#endif

440
User/device/motor_lz.c
View File

@ -1,440 +0,0 @@
/*
灵足电机驱动
灵足电机通信协议：
- CAN 2.0通信接口，波特率1Mbps
- 采用扩展帧格式(29位ID)
- ID格式：Bit28~24(通信类型) + Bit23~8(数据区2) + Bit7~0(目标地址)
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_lz.h"
#include <stdint.h>
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* Private define ----------------------------------------------------------- */
// 灵足电机协议参数
#define LZ_ANGLE_RANGE_RAD (12.57f) /* 角度范围 ±12.57 rad */
#define LZ_VELOCITY_RANGE_RAD_S (20.0f) /* 角速度范围 ±20 rad/s */
#define LZ_TORQUE_RANGE_NM (60.0f) /* 力矩范围 ±60 Nm */
#define LZ_KP_MAX (5000.0f) /* Kp最大值 */
#define LZ_KD_MAX (100.0f) /* Kd最大值 */
#define LZ_RAW_VALUE_MAX (65535) /* 16位原始值最大值 */
#define LZ_TEMP_SCALE (10.0f) /* 温度缩放因子 */
#define LZ_MAX_RECOVER_DIFF_RAD (0.28f)
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
/* Private macro ------------------------------------------------------------ */
MOTOR_LZ_MotionParam_t lz_relax_param = {
.target_angle = 0.0f,
.target_velocity = 0.0f,
.kp = 0.0f,
.kd = 0.0f,
.torque = 0.0f,
};
/* Private typedef ---------------------------------------------------------- */
/* Private variables -------------------------------------------------------- */
static MOTOR_LZ_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
/* Private function prototypes ---------------------------------------------- */
static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can);
static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can);
static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg);
static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id);
static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value);
static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value);
static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc);
static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type);
/* Private functions -------------------------------------------------------- */
/**
* @brief 获取CAN管理器
*/
static MOTOR_LZ_CANManager_t* MOTOR_LZ_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
/**
* @brief 创建CAN管理器
*/
static int8_t MOTOR_LZ_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_LZ_CANManager_t*)BSP_Malloc(sizeof(MOTOR_LZ_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_LZ_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
/**
* @brief 构建扩展ID
*/
static uint32_t MOTOR_LZ_BuildExtID(MOTOR_LZ_CmdType_t cmd_type, uint16_t data2, uint8_t target_id) {
uint32_t ext_id = 0;
ext_id |= ((uint32_t)cmd_type & 0x1F) << 24; // Bit28~24: 通信类型
ext_id |= ((uint32_t)data2 & 0xFFFF) << 8; // Bit23~8: 数据区2
ext_id |= ((uint32_t)target_id & 0xFF); // Bit7~0: 目标地址
return ext_id;
}
/**
* @brief 浮点值转换为原始值（对称范围：-max_value ~ +max_value）
*/
static uint16_t MOTOR_LZ_FloatToRaw(float value, float max_value) {
// 限制范围
if (value > max_value) value = max_value;
if (value < -max_value) value = -max_value;
// 转换为0~65535范围，对应-max_value~max_value
return (uint16_t)((value + max_value) / (2.0f * max_value) * (float)LZ_RAW_VALUE_MAX);
}
/**
* @brief 浮点值转换为原始值（单向范围：0 ~ +max_value）
*/
static uint16_t MOTOR_LZ_FloatToRawPositive(float value, float max_value) {
// 限制范围
if (value > max_value) value = max_value;
if (value < 0.0f) value = 0.0f;
// 转换为0~65535范围，对应0~max_value
return (uint16_t)(value / max_value * (float)LZ_RAW_VALUE_MAX);
}
/**
* @brief 原始值转换为浮点值
*/
static float MOTOR_LZ_RawToFloat(uint16_t raw_value, float max_value) {
// 将0~65535范围转换为-max_value~max_value
return ((float)raw_value / (float)LZ_RAW_VALUE_MAX) * (2.0f * max_value) - max_value;
}
/**
* @brief 发送扩展帧
*/
static int8_t MOTOR_LZ_SendExtFrame(BSP_CAN_t can, uint32_t ext_id, uint8_t *data, uint8_t dlc) {
BSP_CAN_ExtDataFrame_t tx_frame;
tx_frame.id = ext_id;
tx_frame.dlc = dlc;
if (data != NULL) {
memcpy(tx_frame.data, data, dlc);
} else {
memset(tx_frame.data, 0, dlc);
}
return BSP_CAN_TransmitExtDataFrame(can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
/**
* @brief 灵足电机ID解析器
* @param original_id 原始CAN ID（29位扩展帧）
* @param frame_type 帧类型
* @return 解析后的ID（用于队列匹配）
*
* 灵足电机扩展ID格式：
* Bit28~24: 通信类型 (0x1=运控控制, 0x2=反馈数据, 0x3=使能, 0x4=停止, 0x6=设零位)
* Bit23~8: 数据区2 (根据通信类型而定)
* Bit7~0: 目标地址 (目标电机CAN ID)
*/
static uint32_t MOTOR_LZ_IdParser(uint32_t original_id, BSP_CAN_FrameType_t frame_type) {
// 只处理扩展数据帧
if (frame_type != BSP_CAN_FRAME_EXT_DATA) {
return original_id; // 非扩展帧直接返回原始ID
}
// 解析扩展ID各个字段
uint8_t cmd_type = (original_id >> 24) & 0x1F; // Bit28~24: 通信类型
uint16_t data2 = (original_id >> 8) & 0xFFFF; // Bit23~8: 数据区2
uint8_t host_id = (uint8_t)(original_id & 0xFF); // Bit7~0: 主机CAN ID
// 对于反馈数据帧，我们使用特殊的解析规则
if (cmd_type == MOTOR_LZ_CMD_FEEDBACK) {
// 反馈数据的data2字段包含：
// Bit8~15: 当前电机CAN ID
// Bit16~21: 故障信息
// Bit22~23: 模式状态
uint8_t motor_can_id = data2 & 0xFF; // bit8~15: 当前电机CAN ID
// 返回格式化的ID，便于匹配
// 格式：0x02HHMMTT (02=反馈命令, HH=主机ID, MM=电机ID, TT=主机ID)
return (0x02000000) | (host_id << 16) | (motor_can_id << 8) | host_id;
}
// 对于其他命令类型，直接返回原始ID
return original_id;
}
/**
* @brief 解码灵足电机反馈数据
*/
static void MOTOR_LZ_Decode(MOTOR_LZ_t *motor, BSP_CAN_Message_t *msg) {
if (motor == NULL || msg == NULL) return;
uint8_t cmd_type = (msg->original_id >> 24) & 0x1F;
if (cmd_type != MOTOR_LZ_CMD_FEEDBACK) return;
uint16_t id_data2 = (msg->original_id >> 8) & 0xFFFF;
uint8_t motor_can_id = id_data2 & 0xFF;
uint8_t fault_info = (id_data2 >> 8) & 0x3F;
uint8_t mode_state = (id_data2 >> 14) & 0x03;
motor->lz_feedback.motor_can_id = motor_can_id;
motor->lz_feedback.fault.under_voltage = (fault_info & 0x01) != 0;
motor->lz_feedback.fault.driver_fault = (fault_info & 0x02) != 0;
motor->lz_feedback.fault.over_temp = (fault_info & 0x04) != 0;
motor->lz_feedback.fault.encoder_fault = (fault_info & 0x08) != 0;
motor->lz_feedback.fault.stall_overload = (fault_info & 0x10) != 0;
motor->lz_feedback.fault.uncalibrated = (fault_info & 0x20) != 0;
motor->lz_feedback.state = (MOTOR_LZ_State_t)mode_state;
// 反馈解码并自动反向
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
float angle = MOTOR_LZ_RawToFloat(raw_angle, LZ_ANGLE_RANGE_RAD);
uint16_t raw_velocity = (uint16_t)((msg->data[2] << 8) | msg->data[3]);
float velocity = MOTOR_LZ_RawToFloat(raw_velocity, LZ_VELOCITY_RANGE_RAD_S);
uint16_t raw_torque = (uint16_t)((msg->data[4] << 8) | msg->data[5]);
float torque = MOTOR_LZ_RawToFloat(raw_torque, LZ_TORQUE_RANGE_NM);
while (angle <0){
angle += M_2PI;
}
while (angle > M_2PI){
angle -= M_2PI;
}
// 自动反向
if (motor->param.reverse) {
angle = M_2PI - angle;
velocity = -velocity;
torque = -torque;
}
motor->lz_feedback.current_angle = angle;
motor->lz_feedback.current_velocity = velocity;
motor->lz_feedback.current_torque = torque;
uint16_t raw_temp = (uint16_t)((msg->data[6] << 8) | msg->data[7]);
motor->lz_feedback.temperature = (float)raw_temp / LZ_TEMP_SCALE;
motor->motor.feedback.rotor_abs_angle = angle;
motor->motor.feedback.rotor_speed = velocity;
motor->motor.feedback.torque_current = torque;
motor->motor.feedback.temp = (int8_t)motor->lz_feedback.temperature;
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
}
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化灵足电机驱动系统
* @return 设备状态码
*/
int8_t MOTOR_LZ_Init(void) {
// 注册灵足电机专用的ID解析器
return BSP_CAN_RegisterIdParser(MOTOR_LZ_IdParser) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_LZ_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.motor_id == param->motor_id) {
return DEVICE_ERR; // 已注册
}
}
// 检查数量
if (manager->motor_count >= MOTOR_LZ_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
MOTOR_LZ_t *new_motor = (MOTOR_LZ_t*)BSP_Malloc(sizeof(MOTOR_LZ_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(MOTOR_LZ_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
memset(&new_motor->lz_feedback, 0, sizeof(MOTOR_LZ_Feedback_t));
memset(&new_motor->motion_param, 0, sizeof(MOTOR_LZ_MotionParam_t));
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID - 使用解析后的反馈数据ID
// 构建反馈数据的原始扩展ID
// 反馈数据：data2包含电机ID(bit8~15)，target_id是主机ID
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
// 通过ID解析器得到解析后的ID
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
if (BSP_CAN_RegisterId(param->can, parsed_feedback_id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor && motor->param.motor_id == param->motor_id) {
// 获取反馈数据 - 使用解析后的ID
uint32_t original_feedback_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_FEEDBACK, param->motor_id, param->host_id);
uint32_t parsed_feedback_id = MOTOR_LZ_IdParser(original_feedback_id, BSP_CAN_FRAME_EXT_DATA);
BSP_CAN_Message_t msg;
while (BSP_CAN_GetMessage(param->can, parsed_feedback_id, &msg, 0) == BSP_OK) {
MOTOR_LZ_Decode(motor, &msg);
}
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
int8_t MOTOR_LZ_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor) {
if (MOTOR_LZ_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param) {
if (param == NULL || motion_param == NULL) return DEVICE_ERR_NULL;
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
// 自动反向控制
MOTOR_LZ_MotionParam_t send_param = *motion_param;
if (param->reverse) {
send_param.target_angle = -send_param.target_angle;
send_param.target_velocity = -send_param.target_velocity;
send_param.torque = -send_param.torque;
}
memcpy(&motor->motion_param, motion_param, sizeof(MOTOR_LZ_MotionParam_t));
uint16_t raw_torque = MOTOR_LZ_FloatToRaw(send_param.torque, LZ_TORQUE_RANGE_NM);
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_MOTION, raw_torque, param->motor_id);
uint8_t data[8];
uint16_t raw_angle = MOTOR_LZ_FloatToRaw(send_param.target_angle, LZ_ANGLE_RANGE_RAD);
data[0] = (raw_angle >> 8) & 0xFF;
data[1] = raw_angle & 0xFF;
uint16_t raw_velocity = MOTOR_LZ_FloatToRaw(send_param.target_velocity, LZ_VELOCITY_RANGE_RAD_S);
data[2] = (raw_velocity >> 8) & 0xFF;
data[3] = raw_velocity & 0xFF;
uint16_t raw_kp = MOTOR_LZ_FloatToRawPositive(send_param.kp, LZ_KP_MAX);
data[4] = (raw_kp >> 8) & 0xFF;
data[5] = raw_kp & 0xFF;
uint16_t raw_kd = MOTOR_LZ_FloatToRawPositive(send_param.kd, LZ_KD_MAX);
data[6] = (raw_kd >> 8) & 0xFF;
data[7] = raw_kd & 0xFF;
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 使能命令
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_ENABLE, param->host_id, param->motor_id);
// 数据区清零
uint8_t data[8] = {0};
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 停止命令
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_DISABLE, param->host_id, param->motor_id);
// 数据区
uint8_t data[8] = {0};
if (clear_fault) {
data[0] = 1; // Byte[0]=1时清故障
}
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
// 构建扩展ID - 设置零位命令
uint32_t ext_id = MOTOR_LZ_BuildExtID(MOTOR_LZ_CMD_SET_ZERO, param->host_id, param->motor_id);
// 数据区 - Byte[0]=1
uint8_t data[8] = {1, 0, 0, 0, 0, 0, 0, 0};
return MOTOR_LZ_SendExtFrame(param->can, ext_id, data, 8);
}
MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param) {
if (param == NULL) return NULL;
MOTOR_LZ_CANManager_t *manager = MOTOR_LZ_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_LZ_t *motor = manager->motors[i];
if (motor && motor->param.motor_id == param->motor_id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param) {
return MOTOR_LZ_MotionControl(param, &lz_relax_param);
}
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param) {
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}
static MOTOR_LZ_Feedback_t* MOTOR_LZ_GetFeedback(MOTOR_LZ_Param_t *param) {
MOTOR_LZ_t *motor = MOTOR_LZ_GetMotor(param);
if (motor && motor->motor.header.online) {
return &motor->lz_feedback;
}
return NULL;
}

212
User/device/motor_lz.h
View File

@ -1,212 +0,0 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_LZ_MAX_MOTORS 32
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_LZ_RSO0,
MOTOR_LZ_RSO1,
MOTOR_LZ_RSO2,
MOTOR_LZ_RSO3,
MOTOR_LZ_RSO4,
MOTOR_LZ_RSO5,
MOTOR_LZ_RSO6,
} MOTOR_LZ_Module_t;
/* 灵足电机控制模式 */
typedef enum {
MOTOR_LZ_MODE_MOTION = 0x1, /* 运控模式 */
MOTOR_LZ_MODE_CURRENT = 0x2, /* 电流模式 */
MOTOR_LZ_MODE_VELOCITY = 0x3, /* 速度模式 */
MOTOR_LZ_MODE_POSITION = 0x4, /* 位置模式 */
} MOTOR_LZ_ControlMode_t;
/* 灵足电机通信类型 */
typedef enum {
MOTOR_LZ_CMD_MOTION = 0x1, /* 运控模式控制 */
MOTOR_LZ_CMD_FEEDBACK = 0x2, /* 电机反馈数据 */
MOTOR_LZ_CMD_ENABLE = 0x3, /* 电机使能运行 */
MOTOR_LZ_CMD_DISABLE = 0x4, /* 电机停止运行 */
MOTOR_LZ_CMD_SET_ZERO = 0x6, /* 设置电机机械零位 */
} MOTOR_LZ_CmdType_t;
/* 灵足电机运行状态 */
typedef enum {
MOTOR_LZ_STATE_RESET = 0, /* Reset模式[复位] */
MOTOR_LZ_STATE_CALI = 1, /* Cali模式[标定] */
MOTOR_LZ_STATE_MOTOR = 2, /* Motor模式[运行] */
} MOTOR_LZ_State_t;
/* 灵足电机故障信息 */
typedef struct {
bool uncalibrated; /* bit21: 未标定 */
bool stall_overload; /* bit20: 堵转过载故障 */
bool encoder_fault; /* bit19: 磁编码故障 */
bool over_temp; /* bit18: 过温 */
bool driver_fault; /* bit17: 驱动故障 */
bool under_voltage; /* bit16: 欠压故障 */
} MOTOR_LZ_Fault_t;
/* 灵足电机运控参数 */
typedef struct {
float target_angle; /* 目标角度 (-12.57f~12.57f rad) */
float target_velocity; /* 目标角速度 (-20~20 rad/s) */
float kp; /* 位置增益 (0.0~5000.0) */
float kd; /* 微分增益 (0.0~100.0) */
float torque; /* 力矩 (-60~60 Nm) */
} MOTOR_LZ_MotionParam_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can; /* CAN总线 */
uint8_t motor_id; /* 电机CAN ID */
uint8_t host_id; /* 主机CAN ID */
MOTOR_LZ_Module_t module; /* 电机型号 */
bool reverse; /* 是否反向 */
MOTOR_LZ_ControlMode_t mode; /* 控制模式 */
} MOTOR_LZ_Param_t;
/*电机反馈信息扩展*/
typedef struct {
float current_angle; /* 当前角度 (-12.57f~12.57f rad) */
float current_velocity; /* 当前角速度 (-20~20 rad/s) */
float current_torque; /* 当前力矩 (-60~60 Nm) */
float temperature; /* 当前温度 (摄氏度) */
MOTOR_LZ_State_t state; /* 运行状态 */
MOTOR_LZ_Fault_t fault; /* 故障信息 */
uint8_t motor_can_id; /* 当前电机CAN ID */
} MOTOR_LZ_Feedback_t;
/*电机实例*/
typedef struct {
MOTOR_LZ_Param_t param;
MOTOR_t motor;
MOTOR_LZ_Feedback_t lz_feedback; /* 灵足电机特有反馈信息 */
MOTOR_LZ_MotionParam_t motion_param; /* 运控模式参数 */
} MOTOR_LZ_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_LZ_t *motors[MOTOR_LZ_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_LZ_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 初始化灵足电机驱动系统
* @return 设备状态码
*/
int8_t MOTOR_LZ_Init(void);
/**
* @brief 注册一个灵足电机
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Register(MOTOR_LZ_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Update(MOTOR_LZ_Param_t *param);
/**
* @brief 更新所有电机数据
* @return 设备状态码
*/
int8_t MOTOR_LZ_UpdateAll(void);
/**
* @brief 运控模式控制电机
* @param param 电机参数
* @param motion_param 运控参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_MotionControl(MOTOR_LZ_Param_t *param, MOTOR_LZ_MotionParam_t *motion_param);
/**
* @brief 电流(力矩)模式控制电机
* @param param 电机参数
* @param torque 目标力矩 (-60~60 Nm)
* @return 设备状态码
*/
int8_t MOTOR_LZ_TorqueControl(MOTOR_LZ_Param_t *param, float torque);
/**
* @brief 位置模式控制电机
* @param param 电机参数
* @param target_angle 目标角度 (-12.57~12.57 rad)
* @param max_velocity 最大速度 (0~20 rad/s)
* @return 设备状态码
*/
int8_t MOTOR_LZ_PositionControl(MOTOR_LZ_Param_t *param, float target_angle, float max_velocity);
/**
* @brief 速度模式控制电机
* @param param 电机参数
* @param target_velocity 目标速度 (-20~20 rad/s)
* @return 设备状态码
*/
int8_t MOTOR_LZ_VelocityControl(MOTOR_LZ_Param_t *param, float target_velocity);
/**
* @brief 电机使能运行
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Enable(MOTOR_LZ_Param_t *param);
/**
* @brief 电机停止运行
* @param param 电机参数
* @param clear_fault 是否清除故障
* @return 设备状态码
*/
int8_t MOTOR_LZ_Disable(MOTOR_LZ_Param_t *param, bool clear_fault);
/**
* @brief 设置电机机械零位
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_SetZero(MOTOR_LZ_Param_t *param);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return 电机实例指针，失败返回NULL
*/
MOTOR_LZ_t* MOTOR_LZ_GetMotor(MOTOR_LZ_Param_t *param);
/**
* @brief 使电机松弛（发送停止命令）
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Relax(MOTOR_LZ_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param 电机参数
* @return 设备状态码
*/
int8_t MOTOR_LZ_Offline(MOTOR_LZ_Param_t *param);
#ifdef __cplusplus
}
#endif

321
User/device/motor_rm.c
View File

@ -1,321 +0,0 @@
/*
RM电机驱动
*/
/* Includes ----------------------------------------------------------------- */
#include "motor_rm.h"
#include <stdbool.h>
#include <string.h>
#include "bsp/can.h"
#include "bsp/mm.h"
#include "bsp/time.h"
#include "component/user_math.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
#define GM6020_FB_ID_BASE (0x205)
#define GM6020_CTRL_ID_BASE (0x1ff)
#define GM6020_CTRL_ID_EXTAND (0x2ff)
#define M3508_M2006_FB_ID_BASE (0x201)
#define M3508_M2006_CTRL_ID_BASE (0x200)
#define M3508_M2006_CTRL_ID_EXTAND (0x1ff)
#define M3508_M2006_ID_SETTING_ID (0x700)
#define GM6020_MAX_ABS_LSB (30000)
#define M3508_MAX_ABS_LSB (16384)
#define M2006_MAX_ABS_LSB (10000)
#define MOTOR_TX_BUF_SIZE (8)
#define MOTOR_RX_BUF_SIZE (8)
#define MOTOR_ENC_RES (8192) /* 电机编码器分辨率 */
#define MOTOR_CUR_RES (16384) /* 电机转矩电流分辨率 */
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
static MOTOR_RM_CANManager_t *can_managers[BSP_CAN_NUM] = {NULL};
/* Private function -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
static int8_t MOTOR_RM_GetLogicalIndex(uint16_t can_id, MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006:
case MOTOR_M3508:
if (can_id >= M3508_M2006_FB_ID_BASE && can_id <= M3508_M2006_FB_ID_BASE + 7) {
return can_id - M3508_M2006_FB_ID_BASE;
}
break;
case MOTOR_GM6020:
if (can_id >= GM6020_FB_ID_BASE && can_id <= GM6020_FB_ID_BASE + 6) {
return can_id - GM6020_FB_ID_BASE + 4;
}
break;
default:
break;
}
return DEVICE_ERR;
}
static float MOTOR_RM_GetRatio(MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006: return 36.0f;
case MOTOR_M3508: return 3591.0f / 187.0f;
case MOTOR_GM6020: return 1.0f;
default: return 1.0f;
}
}
static int16_t MOTOR_RM_GetLSB(MOTOR_RM_Module_t module) {
switch (module) {
case MOTOR_M2006: return M2006_MAX_ABS_LSB;
case MOTOR_M3508: return M3508_MAX_ABS_LSB;
case MOTOR_GM6020: return GM6020_MAX_ABS_LSB;
default: return DEVICE_ERR;
}
}
static MOTOR_RM_CANManager_t* MOTOR_RM_GetCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return NULL;
return can_managers[can];
}
static int8_t MOTOR_RM_CreateCANManager(BSP_CAN_t can) {
if (can >= BSP_CAN_NUM) return DEVICE_ERR;
if (can_managers[can] != NULL) return DEVICE_OK;
can_managers[can] = (MOTOR_RM_CANManager_t*)BSP_Malloc(sizeof(MOTOR_RM_CANManager_t));
if (can_managers[can] == NULL) return DEVICE_ERR;
memset(can_managers[can], 0, sizeof(MOTOR_RM_CANManager_t));
can_managers[can]->can = can;
return DEVICE_OK;
}
static void Motor_RM_Decode(MOTOR_RM_t *motor, BSP_CAN_Message_t *msg) {
uint16_t raw_angle = (uint16_t)((msg->data[0] << 8) | msg->data[1]);
int16_t raw_speed = (int16_t)((msg->data[2] << 8) | msg->data[3]);
int16_t raw_current = (int16_t)((msg->data[4] << 8) | msg->data[5]);
int16_t lsb = MOTOR_RM_GetLSB(motor->param.module);
float ratio = MOTOR_RM_GetRatio(motor->param.module);
float rotor_angle = raw_angle / (float)MOTOR_ENC_RES * M_2PI;
float rotor_speed = raw_speed;
float torque_current = raw_current * lsb / (float)MOTOR_CUR_RES;
if (motor->param.gear) {
// 多圈累加
int32_t delta = (int32_t)raw_angle - (int32_t)motor->last_raw_angle;
if (delta > (MOTOR_ENC_RES / 2)) {
motor->gearbox_round_count--;
} else if (delta < -(MOTOR_ENC_RES / 2)) {
motor->gearbox_round_count++;
}
motor->last_raw_angle = raw_angle;
float single_turn = rotor_angle;
motor->gearbox_total_angle = (motor->gearbox_round_count * M_2PI + single_turn) / ratio;
// 输出轴多圈绝对值
motor->feedback.rotor_abs_angle = motor->gearbox_total_angle;
motor->feedback.rotor_speed = rotor_speed / ratio;
motor->feedback.torque_current = torque_current * ratio;
} else {
// 非gear模式，直接用转子单圈
motor->gearbox_round_count = 0;
motor->last_raw_angle = raw_angle;
motor->gearbox_total_angle = 0.0f;
motor->feedback.rotor_abs_angle = rotor_angle;
motor->feedback.rotor_speed = rotor_speed;
motor->feedback.torque_current = torque_current;
}
while (motor->feedback.rotor_abs_angle < 0) {
motor->feedback.rotor_abs_angle += M_2PI;
}
while (motor->feedback.rotor_abs_angle >= M_2PI) {
motor->feedback.rotor_abs_angle -= M_2PI;
}
if (motor->motor.reverse) {
motor->feedback.rotor_abs_angle = M_2PI - motor->feedback.rotor_abs_angle;
motor->feedback.rotor_speed = -motor->feedback.rotor_speed;
motor->feedback.torque_current = -motor->feedback.torque_current;
}
motor->feedback.temp = msg->data[6];
}
/* Exported functions ------------------------------------------------------- */
int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
if (MOTOR_RM_CreateCANManager(param->can) != DEVICE_OK) return DEVICE_ERR;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR;
// 检查是否已注册
for (int i = 0; i < manager->motor_count; i++) {
if (manager->motors[i] && manager->motors[i]->param.id == param->id) {
return DEVICE_ERR_INITED;
}
}
// 检查数量
if (manager->motor_count >= MOTOR_RM_MAX_MOTORS) return DEVICE_ERR;
// 创建新电机实例
MOTOR_RM_t *new_motor = (MOTOR_RM_t*)BSP_Malloc(sizeof(MOTOR_RM_t));
if (new_motor == NULL) return DEVICE_ERR;
memcpy(&new_motor->param, param, sizeof(MOTOR_RM_Param_t));
memset(&new_motor->motor, 0, sizeof(MOTOR_t));
new_motor->motor.reverse = param->reverse;
// 注册CAN接收ID
if (BSP_CAN_RegisterId(param->can, param->id, 3) != BSP_OK) {
BSP_Free(new_motor);
return DEVICE_ERR;
}
manager->motors[manager->motor_count] = new_motor;
manager->motor_count++;
return DEVICE_OK;
}
int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_RM_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
BSP_CAN_Message_t rx_msg;
if (BSP_CAN_GetMessage(param->can, param->id, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) != BSP_OK) {
uint64_t now_time = BSP_TIME_Get();
if (now_time - motor->motor.header.last_online_time > 1000) {
motor->motor.header.online = false;
return DEVICE_ERR_NO_DEV;
}
return DEVICE_ERR;
}
motor->motor.header.online = true;
motor->motor.header.last_online_time = BSP_TIME_Get();
Motor_RM_Decode(motor, &rx_msg);
motor->motor.feedback = motor->feedback;
return DEVICE_OK;
}
}
return DEVICE_ERR_NO_DEV;
}
int8_t MOTOR_RM_UpdateAll(void) {
int8_t ret = DEVICE_OK;
for (int can = 0; can < BSP_CAN_NUM; can++) {
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager((BSP_CAN_t)can);
if (manager == NULL) continue;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_RM_t *motor = manager->motors[i];
if (motor != NULL) {
if (MOTOR_RM_Update(&motor->param) != DEVICE_OK) {
ret = DEVICE_ERR;
}
}
}
}
return ret;
}
int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
if (value > 1.0f) value = 1.0f;
if (value < -1.0f) value = -1.0f;
if (param->reverse){
value = -value;
}
MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param);
if (motor == NULL) return DEVICE_ERR_NO_DEV;
int8_t logical_index = MOTOR_RM_GetLogicalIndex(param->id, param->module);
if (logical_index < 0) return DEVICE_ERR;
MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg;
int16_t output_value = (int16_t)(value * (float)MOTOR_RM_GetLSB(param->module));
output_msg->output[logical_index] = output_value;
return DEVICE_OK;
}
int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param) {
if (param == NULL) return DEVICE_ERR_NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return DEVICE_ERR_NO_DEV;
MOTOR_RM_MsgOutput_t *output_msg = &manager->output_msg;
BSP_CAN_StdDataFrame_t tx_frame;
uint16_t id = param->id;
switch (id) {
case M3508_M2006_FB_ID_BASE:
case M3508_M2006_FB_ID_BASE+1:
case M3508_M2006_FB_ID_BASE+2:
case M3508_M2006_FB_ID_BASE+3:
tx_frame.id = M3508_M2006_CTRL_ID_BASE;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
for (int i = 0; i < 4; i++) {
tx_frame.data[i*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
tx_frame.data[i*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
}
break;
case M3508_M2006_FB_ID_BASE+4:
case M3508_M2006_FB_ID_BASE+5:
case M3508_M2006_FB_ID_BASE+6:
case M3508_M2006_FB_ID_BASE+7:
tx_frame.id = M3508_M2006_CTRL_ID_EXTAND;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
for (int i = 4; i < 8; i++) {
tx_frame.data[(i-4)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
tx_frame.data[(i-4)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
}
break;
case GM6020_FB_ID_BASE+4:
case GM6020_FB_ID_BASE+5:
case GM6020_FB_ID_BASE+6:
tx_frame.id = GM6020_CTRL_ID_EXTAND;
tx_frame.dlc = MOTOR_TX_BUF_SIZE;
for (int i = 8; i < 11; i++) {
tx_frame.data[(i-8)*2] = (uint8_t)((output_msg->output[i] >> 8) & 0xFF);
tx_frame.data[(i-8)*2+1] = (uint8_t)(output_msg->output[i] & 0xFF);
}
tx_frame.data[6] = 0;
tx_frame.data[7] = 0;
break;
default:
return DEVICE_ERR;
}
return BSP_CAN_TransmitStdDataFrame(param->can, &tx_frame) == BSP_OK ? DEVICE_OK : DEVICE_ERR;
}
MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param) {
if (param == NULL) return NULL;
MOTOR_RM_CANManager_t *manager = MOTOR_RM_GetCANManager(param->can);
if (manager == NULL) return NULL;
for (int i = 0; i < manager->motor_count; i++) {
MOTOR_RM_t *motor = manager->motors[i];
if (motor && motor->param.id == param->id) {
return motor;
}
}
return NULL;
}
int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param) {
return MOTOR_RM_SetOutput(param, 0.0f);
}
int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param) {
MOTOR_RM_t *motor = MOTOR_RM_GetMotor(param);
if (motor) {
motor->motor.header.online = false;
return DEVICE_OK;
}
return DEVICE_ERR_NO_DEV;
}

132
User/device/motor_rm.h
View File

@ -1,132 +0,0 @@
#pragma once
#include "motor.h"
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "device/device.h"
#include "device/motor.h"
#include "bsp/can.h"
/* Exported constants ------------------------------------------------------- */
#define MOTOR_RM_MAX_MOTORS 11
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
typedef enum {
MOTOR_M2006,
MOTOR_M3508,
MOTOR_GM6020,
} MOTOR_RM_Module_t;
/*一个can最多控制11个电机*/
typedef union {
int16_t output[MOTOR_RM_MAX_MOTORS];
struct {
int16_t m3508_m2006_id201;
int16_t m3508_m2006_id202;
int16_t m3508_m2006_id203;
int16_t m3508_m2006_id204;
int16_t m3508_m2006_gm6020_id205;
int16_t m3508_m2006_gm6020_id206;
int16_t m3508_m2006_gm6020_id207;
int16_t m3508_m2006_gm6020_id208;
int16_t gm6020_id209;
int16_t gm6020_id20A;
int16_t gm6020_id20B;
} named;
} MOTOR_RM_MsgOutput_t;
/*每个电机需要的参数*/
typedef struct {
BSP_CAN_t can;
uint16_t id;
MOTOR_RM_Module_t module;
bool reverse;
bool gear;
} MOTOR_RM_Param_t;
typedef MOTOR_Feedback_t MOTOR_RM_Feedback_t;
typedef struct {
MOTOR_RM_Param_t param;
MOTOR_RM_Feedback_t feedback;
MOTOR_t motor;
// 多圈相关变量，仅gear模式下有效
uint16_t last_raw_angle;
int32_t gearbox_round_count;
float gearbox_total_angle;
} MOTOR_RM_t;
/*CAN管理器，管理一个CAN总线上所有的电机*/
typedef struct {
BSP_CAN_t can;
MOTOR_RM_MsgOutput_t output_msg;
MOTOR_RM_t *motors[MOTOR_RM_MAX_MOTORS];
uint8_t motor_count;
} MOTOR_RM_CANManager_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 注册一个RM电机
* @param param 电机参数
* @return
*/
int8_t MOTOR_RM_Register(MOTOR_RM_Param_t *param);
/**
* @brief 更新指定电机数据
* @param param 电机参数
* @return
*/
int8_t MOTOR_RM_Update(MOTOR_RM_Param_t *param);
/**
* @brief 设置一个电机的输出
* @param param 电机参数
* @param value 输出值，范围[-1.0, 1.0]
* @return
*/
int8_t MOTOR_RM_SetOutput(MOTOR_RM_Param_t *param, float value);
/**
* @brief 发送控制命令到电机，注意一个CAN可以控制多个电机，所以只需要发送一次即可
* @param param 电机参数
* @return
*/
int8_t MOTOR_RM_Ctrl(MOTOR_RM_Param_t *param);
/**
* @brief 获取指定电机的实例指针
* @param param 电机参数
* @return
*/
MOTOR_RM_t* MOTOR_RM_GetMotor(MOTOR_RM_Param_t *param);
/**
* @brief 使电机松弛（设置输出为0）
* @param param
* @return
*/
int8_t MOTOR_RM_Relax(MOTOR_RM_Param_t *param);
/**
* @brief 使电机离线（设置在线状态为false）
* @param param
* @return
*/
int8_t MOTOR_RM_Offine(MOTOR_RM_Param_t *param);
/**
* @brief
* @param
* @return
*/
int8_t MOTOR_RM_UpdateAll(void);
#ifdef __cplusplus
}
#endif

38
User/module/config.c
View File

@ -1,38 +0,0 @@
/*
* 配置相关
*/
/* 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
View File

@ -1,38 +0,0 @@
/*
* 配置相关
*/
#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
View File

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

96
User/task/atti_esti.c
View File

@ -1,96 +0,0 @@
/*
atti_esti Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "bsp/mm.h"
#include "bsp/pwm.h"
#include "bsp/gpio.h"
#include "component/ahrs.h"
#include "component/pid.h"
#include "device/bmi088.h"
#include "task/user_task.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
static BMI088_t bmi088 __attribute__((section(".dma_buffer"))) __attribute__((aligned(32)));
AHRS_t gimbal_ahrs;
AHRS_Magn_t magn;
AHRS_Eulr_t eulr_to_send;
KPID_t imu_temp_ctrl_pid;
BMI088_Cali_t cali_bmi088= {
.gyro_offset = {-0.00149519544f,-0.00268901442f,0.00169837975f},
};
static const KPID_Params_t imu_temp_ctrl_pid_param = {
.k = 0.3f,
.p = 1.0f,
.i = 0.01f,
.d = 0.0f,
.i_limit = 1.0f,
.out_limit = 1.0f,
};
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_atti_esti(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / ATTI_ESTI_FREQ;
osDelay(ATTI_ESTI_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
BMI088_Init(&bmi088,&cali_bmi088);
AHRS_Init(&gimbal_ahrs, &magn, BMI088_GetUpdateFreq(&bmi088));
PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
BMI088_WaitNew();
BMI088_AcclStartDmaRecv();
BMI088_AcclWaitDmaCplt();
BMI088_GyroStartDmaRecv();
BMI088_GyroWaitDmaCplt();
/* 锁住RTOS内核防止数据解析过程中断，造成错误 */
osKernelLock();
/* 接收完所有数据后，把数据从原始字节加工成方便计算的数据 */
BMI088_ParseAccl(&bmi088);
BMI088_ParseGyro(&bmi088);
// IST8310_Parse(&ist8310);
/* 根据设备接收到的数据进行姿态解析 */
AHRS_Update(&gimbal_ahrs, &bmi088.accl, &bmi088.gyro, &magn);
/* 根据解析出来的四元数计算欧拉角 */
AHRS_GetEulr(&eulr_to_send, &gimbal_ahrs);
osKernelUnlock();
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

21
User/task/config.yaml
View File

@ -1,21 +0,0 @@
- delay: 0
description: ''
freq_control: true
frequency: 500.0
function: Task_ai
name: ai
stack: 256
- delay: 0
description: ''
freq_control: true
frequency: 500.0
function: Task_rc
name: rc
stack: 256
- delay: 0
description: ''
freq_control: true
frequency: 500.0
function: Task_atti_esti
name: atti_esti
stack: 256

44
User/task/init.c
View File

@ -1,44 +0,0 @@
/*
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);
task_runtime.thread.rc = osThreadNew(Task_rc, NULL, &attr_rc);
task_runtime.thread.atti_esti = osThreadNew(Task_atti_esti, NULL, &attr_atti_esti);
// 创建消息队列
/* USER MESSAGE BEGIN */
task_runtime.msgq.user_msg= osMessageQueueNew(2u, 10, NULL);
/* USER MESSAGE END */
osKernelUnlock(); // 解锁内核
osThreadTerminate(osThreadGetId()); // 任务完成后结束自身
}

56
User/task/rc.c
View File

@ -1,56 +0,0 @@
/*
rc Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/dr16.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* STM32H7: 整个DR16结构体放在DMA可访问的SRAM区域 */
static DR16_t dr16 __attribute__((section(".dma_buffer"))) __attribute__((aligned(32)));
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
void Task_rc(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / RC_FREQ;
osDelay(RC_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
/* 初始化DR16遥控器 */
DR16_Init(&dr16);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
/* 启动DMA接收 */
DR16_StartDmaRecv(&dr16);
/* 等待DMA接收完成，超时时间20ms */
if (DR16_WaitDmaCplt(20)) {
/* DMA接收成功，解析数据 */
DR16_ParseData(&dr16);
} else {
/* DMA接收超时，标记离线 */
DR16_Offline(&dr16);
}
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

26
User/task/user_task.c
View File

@ -1,26 +0,0 @@
#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,
};
const osThreadAttr_t attr_rc = {
.name = "rc",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_atti_esti = {
.name = "atti_esti",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};

96
User/task/user_task.h
View File

@ -1,96 +0,0 @@
#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)
#define RC_FREQ (500.0)
#define ATTI_ESTI_FREQ (500.0)
/* 任务初始化延时ms */
#define TASK_INIT_DELAY (100u)
#define AI_INIT_DELAY (0)
#define RC_INIT_DELAY (0)
#define ATTI_ESTI_INIT_DELAY (0)
/* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* 任务运行时结构体 */
typedef struct {
/* 各任务，也可以叫做线程 */
struct {
osThreadId_t ai;
osThreadId_t rc;
osThreadId_t atti_esti;
} 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;
UBaseType_t rc;
UBaseType_t atti_esti;
} stack_water_mark;
/* 各任务运行频率 */
struct {
float ai;
float rc;
float atti_esti;
} freq;
/* 任务最近运行时间 */
struct {
float ai;
float rc;
float atti_esti;
} last_up_time;
} Task_Runtime_t;
/* 任务运行时结构体 */
extern Task_Runtime_t task_runtime;
/* 初始化任务句柄 */
extern const osThreadAttr_t attr_init;
extern const osThreadAttr_t attr_ai;
extern const osThreadAttr_t attr_rc;
extern const osThreadAttr_t attr_atti_esti;
/* 任务函数声明 */
void Task_Init(void *argument);
void Task_ai(void *argument);
void Task_rc(void *argument);
void Task_atti_esti(void *argument);
#ifdef __cplusplus
}
#endif

186
balance_infantry.ioc
View File

@ -313,7 +313,6 @@ Dma.USART3_TX.10.SyncEnable=DISABLE
Dma.USART3_TX.10.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
Dma.USART3_TX.10.SyncRequestNumber=1
Dma.USART3_TX.10.SyncSignalID=NONE
FDCAN1.AutoRetransmission=ENABLE
FDCAN1.CalculateBaudRateNominal=999999
FDCAN1.CalculateTimeBitNominal=1000
FDCAN1.CalculateTimeQuantumNominal=90.90909090909092
@ -322,8 +321,8 @@ FDCAN1.DataSyncJumpWidth=10
FDCAN1.DataTimeSeg1=9
FDCAN1.DataTimeSeg2=1
FDCAN1.ExtFiltersNbr=1
FDCAN1.FrameFormat=FDCAN_FRAME_CLASSIC
FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,NominalTimeSeg1,FrameFormat,NominalSyncJumpWidth,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,MessageRAMOffset,ExtFiltersNbr,RxFifo0ElmtSize,RxFifo1ElmtsNbr,RxFifo1ElmtSize,RxBuffersNbr,RxBufferSize,TxElmtSize,NominalTimeSeg2,DataPrescaler,AutoRetransmission
FDCAN1.FrameFormat=FDCAN_FRAME_FD_BRS
FDCAN1.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,NominalTimeSeg1,FrameFormat,NominalSyncJumpWidth,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,MessageRAMOffset,ExtFiltersNbr,RxFifo0ElmtSize,RxFifo1ElmtsNbr,RxFifo1ElmtSize,RxBuffersNbr,RxBufferSize,TxElmtSize,NominalTimeSeg2,DataPrescaler
FDCAN1.MessageRAMOffset=0
FDCAN1.NominalPrescaler=5
FDCAN1.NominalSyncJumpWidth=10
@ -338,7 +337,6 @@ FDCAN1.RxFifo1ElmtsNbr=0
FDCAN1.StdFiltersNbr=1
FDCAN1.TxElmtSize=FDCAN_DATA_BYTES_64
FDCAN1.TxFifoQueueElmtsNbr=23
FDCAN2.AutoRetransmission=ENABLE
FDCAN2.CalculateBaudRateNominal=999999
FDCAN2.CalculateTimeBitNominal=1000
FDCAN2.CalculateTimeQuantumNominal=90.90909090909092
@ -347,8 +345,8 @@ FDCAN2.DataSyncJumpWidth=10
FDCAN2.DataTimeSeg1=9
FDCAN2.DataTimeSeg2=1
FDCAN2.ExtFiltersNbr=1
FDCAN2.FrameFormat=FDCAN_FRAME_CLASSIC
FDCAN2.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,MessageRAMOffset,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,FrameFormat,NominalSyncJumpWidth,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,ExtFiltersNbr,RxFifo0ElmtSize,RxFifo1ElmtsNbr,RxFifo1ElmtSize,RxBuffersNbr,RxBufferSize,TxBuffersNbr,TxElmtSize,NominalTimeSeg1,NominalTimeSeg2,DataPrescaler,AutoRetransmission
FDCAN2.FrameFormat=FDCAN_FRAME_FD_BRS
FDCAN2.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,MessageRAMOffset,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,FrameFormat,NominalSyncJumpWidth,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,ExtFiltersNbr,RxFifo0ElmtSize,RxFifo1ElmtsNbr,RxFifo1ElmtSize,RxBuffersNbr,RxBufferSize,TxBuffersNbr,TxElmtSize,NominalTimeSeg1,NominalTimeSeg2,DataPrescaler
FDCAN2.MessageRAMOffset=831
FDCAN2.NominalPrescaler=5
FDCAN2.NominalSyncJumpWidth=10
@ -364,7 +362,6 @@ FDCAN2.StdFiltersNbr=1
FDCAN2.TxBuffersNbr=0
FDCAN2.TxElmtSize=FDCAN_DATA_BYTES_64
FDCAN2.TxFifoQueueElmtsNbr=23
FDCAN3.AutoRetransmission=ENABLE
FDCAN3.CalculateBaudRateNominal=999999
FDCAN3.CalculateTimeBitNominal=1000
FDCAN3.CalculateTimeQuantumNominal=90.90909090909092
@ -373,8 +370,8 @@ FDCAN3.DataSyncJumpWidth=10
FDCAN3.DataTimeSeg1=9
FDCAN3.DataTimeSeg2=1
FDCAN3.ExtFiltersNbr=1
FDCAN3.FrameFormat=FDCAN_FRAME_CLASSIC
FDCAN3.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,MessageRAMOffset,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,FrameFormat,NominalSyncJumpWidth,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,ExtFiltersNbr,RxFifo0ElmtSize,RxFifo1ElmtsNbr,RxFifo1ElmtSize,RxBuffersNbr,RxBufferSize,TxBuffersNbr,TxElmtSize,NominalTimeSeg1,NominalTimeSeg2,DataPrescaler,Mode,AutoRetransmission
FDCAN3.FrameFormat=FDCAN_FRAME_FD_BRS
FDCAN3.IPParameters=CalculateTimeQuantumNominal,CalculateTimeBitNominal,CalculateBaudRateNominal,MessageRAMOffset,RxFifo0ElmtsNbr,TxFifoQueueElmtsNbr,NominalPrescaler,StdFiltersNbr,FrameFormat,NominalSyncJumpWidth,DataSyncJumpWidth,DataTimeSeg1,DataTimeSeg2,ExtFiltersNbr,RxFifo0ElmtSize,RxFifo1ElmtsNbr,RxFifo1ElmtSize,RxBuffersNbr,RxBufferSize,TxBuffersNbr,TxElmtSize,NominalTimeSeg1,NominalTimeSeg2,DataPrescaler,Mode
FDCAN3.MessageRAMOffset=1662
FDCAN3.Mode=FDCAN_MODE_NORMAL
FDCAN3.NominalPrescaler=5
@ -391,9 +388,8 @@ FDCAN3.StdFiltersNbr=1
FDCAN3.TxBuffersNbr=0
FDCAN3.TxElmtSize=FDCAN_DATA_BYTES_64
FDCAN3.TxFifoQueueElmtsNbr=23
FREERTOS.IPParameters=Tasks01,configTOTAL_HEAP_SIZE
FREERTOS.IPParameters=Tasks01
FREERTOS.Tasks01=defaultTask,24,128,StartDefaultTask,Default,NULL,Dynamic,NULL,NULL
FREERTOS.configTOTAL_HEAP_SIZE=0x10000
File.Version=6
GPIO.groupedBy=Group By Peripherals
KeepUserPlacement=false
@ -405,22 +401,23 @@ Mcu.IP0=ADC1
Mcu.IP1=BDMA
Mcu.IP10=MEMORYMAP
Mcu.IP11=NVIC
Mcu.IP12=RCC
Mcu.IP13=SPI2
Mcu.IP14=SPI6
Mcu.IP15=SYS
Mcu.IP16=TIM1
Mcu.IP17=TIM2
Mcu.IP18=TIM3
Mcu.IP19=TIM12
Mcu.IP12=OCTOSPI1
Mcu.IP13=RCC
Mcu.IP14=SPI2
Mcu.IP15=SPI6
Mcu.IP16=SYS
Mcu.IP17=TIM1
Mcu.IP18=TIM2
Mcu.IP19=TIM3
Mcu.IP2=CORTEX_M7
Mcu.IP20=UART5
Mcu.IP21=UART7
Mcu.IP22=USART1
Mcu.IP23=USART2
Mcu.IP24=USART3
Mcu.IP25=USART10
Mcu.IP26=USB_OTG_HS
Mcu.IP20=TIM12
Mcu.IP21=UART5
Mcu.IP22=UART7
Mcu.IP23=USART1
Mcu.IP24=USART2
Mcu.IP25=USART3
Mcu.IP26=USART10
Mcu.IP27=USB_OTG_HS
Mcu.IP3=DAC1
Mcu.IP4=DEBUG
Mcu.IP5=DMA
@ -428,65 +425,71 @@ Mcu.IP6=FDCAN1
Mcu.IP7=FDCAN2
Mcu.IP8=FDCAN3
Mcu.IP9=FREERTOS
Mcu.IPNb=27
Mcu.IPNb=28
Mcu.Name=STM32H723VGTx
Mcu.Package=LQFP100
Mcu.Pin0=PE2
Mcu.Pin1=PE3
Mcu.Pin10=PC3_C
Mcu.Pin11=PA0
Mcu.Pin12=PA2
Mcu.Pin13=PA5
Mcu.Pin14=PA7
Mcu.Pin15=PC4
Mcu.Pin16=PB1
Mcu.Pin17=PE7
Mcu.Pin18=PE8
Mcu.Pin19=PE9
Mcu.Pin12=PA1
Mcu.Pin13=PA2
Mcu.Pin14=PA3
Mcu.Pin15=PA5
Mcu.Pin16=PA7
Mcu.Pin17=PC4
Mcu.Pin18=PB0
Mcu.Pin19=PB1
Mcu.Pin2=PC13
Mcu.Pin20=PE10
Mcu.Pin21=PE11
Mcu.Pin22=PE12
Mcu.Pin23=PE13
Mcu.Pin24=PE15
Mcu.Pin25=PB10
Mcu.Pin26=PB11
Mcu.Pin27=PB13
Mcu.Pin28=PB14
Mcu.Pin29=PB15
Mcu.Pin20=PB2
Mcu.Pin21=PE7
Mcu.Pin22=PE8
Mcu.Pin23=PE9
Mcu.Pin24=PE10
Mcu.Pin25=PE11
Mcu.Pin26=PE12
Mcu.Pin27=PE13
Mcu.Pin28=PE15
Mcu.Pin29=PB10
Mcu.Pin3=PC14-OSC32_IN
Mcu.Pin30=PD8
Mcu.Pin31=PD9
Mcu.Pin32=PD12
Mcu.Pin33=PD13
Mcu.Pin34=PA9
Mcu.Pin35=PA10
Mcu.Pin36=PA11
Mcu.Pin37=PA12
Mcu.Pin38=PA13(JTMS/SWDIO)
Mcu.Pin39=PA14(JTCK/SWCLK)
Mcu.Pin30=PB11
Mcu.Pin31=PB13
Mcu.Pin32=PB14
Mcu.Pin33=PB15
Mcu.Pin34=PD8
Mcu.Pin35=PD9
Mcu.Pin36=PD11
Mcu.Pin37=PD12
Mcu.Pin38=PD13
Mcu.Pin39=PA9
Mcu.Pin4=PC15-OSC32_OUT
Mcu.Pin40=PA15(JTDI)
Mcu.Pin41=PC12
Mcu.Pin42=PD0
Mcu.Pin43=PD1
Mcu.Pin44=PD2
Mcu.Pin45=PD4
Mcu.Pin46=PD5
Mcu.Pin47=PD6
Mcu.Pin48=PB3(JTDO/TRACESWO)
Mcu.Pin49=PB5
Mcu.Pin40=PA10
Mcu.Pin41=PA11
Mcu.Pin42=PA12
Mcu.Pin43=PA13(JTMS/SWDIO)
Mcu.Pin44=PA14(JTCK/SWCLK)
Mcu.Pin45=PA15(JTDI)
Mcu.Pin46=PC12
Mcu.Pin47=PD0
Mcu.Pin48=PD1
Mcu.Pin49=PD2
Mcu.Pin5=PH0-OSC_IN
Mcu.Pin50=PB6
Mcu.Pin51=VP_DAC1_VS_DACI2
Mcu.Pin52=VP_FREERTOS_VS_CMSIS_V2
Mcu.Pin53=VP_SYS_VS_tim4
Mcu.Pin54=VP_MEMORYMAP_VS_MEMORYMAP
Mcu.Pin50=PD4
Mcu.Pin51=PD5
Mcu.Pin52=PD6
Mcu.Pin53=PB3(JTDO/TRACESWO)
Mcu.Pin54=PB5
Mcu.Pin55=PB6
Mcu.Pin56=VP_DAC1_VS_DACI2
Mcu.Pin57=VP_FREERTOS_VS_CMSIS_V2
Mcu.Pin58=VP_OCTOSPI1_VS_octo
Mcu.Pin59=VP_SYS_VS_tim23
Mcu.Pin6=PH1-OSC_OUT
Mcu.Pin60=VP_MEMORYMAP_VS_MEMORYMAP
Mcu.Pin7=PC0
Mcu.Pin8=PC1
Mcu.Pin9=PC2_C
Mcu.PinsNb=55
Mcu.PinsNb=61
Mcu.ThirdPartyNb=0
Mcu.UserConstants=
Mcu.UserName=STM32H723VGTx
@ -509,6 +512,7 @@ NVIC.DMA2_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream4_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA2_Stream5_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DebugMonitor_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
NVIC.EXTI15_10_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.FDCAN1_IT0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.FDCAN1_IT1_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.FDCAN2_IT0_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
@ -529,9 +533,9 @@ NVIC.SavedPendsvIrqHandlerGenerated=true
NVIC.SavedSvcallIrqHandlerGenerated=true
NVIC.SavedSystickIrqHandlerGenerated=true
NVIC.SysTick_IRQn=true\:15\:0\:false\:false\:false\:true\:false\:true\:false
NVIC.TIM4_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
NVIC.TimeBase=TIM4_IRQn
NVIC.TimeBaseIP=TIM4
NVIC.TIM23_IRQn=true\:15\:0\:false\:false\:true\:false\:false\:true\:true
NVIC.TimeBase=TIM23_IRQn
NVIC.TimeBaseIP=TIM23
NVIC.UART5_IRQn=true\:5\:0\:false\:false\:true\:true\:true\:true\:true
NVIC.UART7_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
NVIC.USART10_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
@ -541,6 +545,9 @@ NVIC.USART3_IRQn=true\:5\:0\:true\:false\:true\:true\:true\:true\:true
NVIC.UsageFault_IRQn=true\:0\:0\:false\:false\:true\:false\:false\:false\:false
PA0.Locked=true
PA0.Signal=S_TIM2_CH1_ETR
PA1.Locked=true
PA1.Mode=OCTOSPI1_IOL_Port1L
PA1.Signal=OCTOSPIM_P1_IO3
PA10.Locked=true
PA10.Mode=Asynchronous
PA10.Signal=USART1_RX
@ -558,6 +565,9 @@ PA15(JTDI).Locked=true
PA15(JTDI).Signal=GPIO_Input
PA2.Locked=true
PA2.Signal=S_TIM2_CH3
PA3.Locked=true
PA3.Mode=OCTOSPI1_IOL_Port1L
PA3.Signal=OCTOSPIM_P1_IO2
PA5.Locked=true
PA5.Signal=ADCx_INP19
PA7.Mode=TX_Only_Simplex_Unidirect_Master
@ -565,6 +575,9 @@ PA7.Signal=SPI6_MOSI
PA9.Locked=true
PA9.Mode=Asynchronous
PA9.Signal=USART1_TX
PB0.Locked=true
PB0.Mode=OCTOSPI1_IOL_Port1L
PB0.Signal=OCTOSPIM_P1_IO1
PB1.Locked=true
PB1.Signal=S_TIM3_CH4
PB10.GPIOParameters=GPIO_Label
@ -584,6 +597,9 @@ PB14.Mode=Hardware Flow Control (RS485)
PB14.Signal=USART3_DE
PB15.Locked=true
PB15.Signal=S_TIM12_CH2
PB2.Locked=true
PB2.Mode=O1_P1_CLK
PB2.Signal=OCTOSPIM_P1_CLK
PB3(JTDO/TRACESWO).Mode=TX_Only_Simplex_Unidirect_Master
PB3(JTDO/TRACESWO).Signal=SPI6_SCK
PB5.Locked=true
@ -593,7 +609,7 @@ PB6.Locked=true
PB6.Mode=FDCAN_Activate
PB6.Signal=FDCAN2_TX
PC0.GPIOParameters=GPIO_Speed,PinState,GPIO_Label
PC0.GPIO_Label=ACC_CS
PC0.GPIO_Label=ACCL_CS
PC0.GPIO_Speed=GPIO_SPEED_FREQ_VERY_HIGH
PC0.Locked=true
PC0.PinState=GPIO_PIN_SET
@ -607,8 +623,10 @@ PC12.Signal=UART5_TX
PC13.GPIOParameters=PinState,GPIO_Label
PC13.GPIO_Label=POWER_24V_2
PC13.Locked=true
PC13.PinState=GPIO_PIN_SET
PC13.PinState=GPIO_PIN_RESET
PC13.Signal=GPIO_Output
PC14-OSC32_IN.GPIOParameters=GPIO_Label
PC14-OSC32_IN.GPIO_Label=POWER_24_EN\!
PC14-OSC32_IN.Locked=true
PC14-OSC32_IN.Signal=GPIO_Output
PC15-OSC32_OUT.GPIOParameters=PinState,GPIO_Label
@ -633,6 +651,9 @@ PD0.Signal=FDCAN1_RX
PD1.Locked=true
PD1.Mode=FDCAN_Activate
PD1.Signal=FDCAN1_TX
PD11.Locked=true
PD11.Mode=OCTOSPI1_IOL_Port1L
PD11.Signal=OCTOSPIM_P1_IO0
PD12.Mode=FDCAN_Activate
PD12.Signal=FDCAN3_RX
PD13.Locked=true
@ -654,7 +675,7 @@ PD8.Signal=USART3_TX
PD9.Mode=Asynchronous
PD9.Signal=USART3_RX
PE10.GPIOParameters=GPIO_PuPd,GPIO_Label
PE10.GPIO_Label=ACC_INT
PE10.GPIO_Label=ACCL_INT
PE10.GPIO_PuPd=GPIO_NOPULL
PE10.Locked=true
PE10.Signal=GPXTI10
@ -699,7 +720,7 @@ ProjectManager.DeviceId=STM32H723VGTx
ProjectManager.FirmwarePackage=STM32Cube FW_H7 V1.12.1
ProjectManager.FreePins=false
ProjectManager.HalAssertFull=false
ProjectManager.HeapSize=0x1000
ProjectManager.HeapSize=0x200
ProjectManager.KeepUserCode=true
ProjectManager.LastFirmware=true
ProjectManager.LibraryCopy=0
@ -711,7 +732,7 @@ ProjectManager.ProjectFileName=balance_infantry.ioc
ProjectManager.ProjectName=balance_infantry
ProjectManager.ProjectStructure=
ProjectManager.RegisterCallBack=
ProjectManager.StackSize=0x2000
ProjectManager.StackSize=0x800
ProjectManager.TargetToolchain=CMake
ProjectManager.ToolChainLocation=
ProjectManager.UAScriptAfterPath=
@ -898,7 +919,8 @@ VP_FREERTOS_VS_CMSIS_V2.Mode=CMSIS_V2
VP_FREERTOS_VS_CMSIS_V2.Signal=FREERTOS_VS_CMSIS_V2
VP_MEMORYMAP_VS_MEMORYMAP.Mode=CurAppReg
VP_MEMORYMAP_VS_MEMORYMAP.Signal=MEMORYMAP_VS_MEMORYMAP
VP_SYS_VS_tim4.Mode=TIM4
VP_SYS_VS_tim4.Signal=SYS_VS_tim4
VP_OCTOSPI1_VS_octo.Mode=octo_mode
VP_OCTOSPI1_VS_octo.Signal=OCTOSPI1_VS_octo
VP_SYS_VS_tim23.Mode=TIM23
VP_SYS_VS_tim23.Signal=SYS_VS_tim23
board=custom
rtos.0.ip=FREERTOS

2
cmake/stm32cubemx/CMakeLists.txt
View File

@ -31,6 +31,7 @@ set(MX_Application_Src
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/dac.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/dma.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/fdcan.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/octospi.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/spi.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/tim.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Core/Src/usart.c
@ -69,6 +70,7 @@ set(STM32_Drivers_Src
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dac.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_dac_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_fdcan.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_ospi.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_spi.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_spi_ex.c
${CMAKE_CURRENT_SOURCE_DIR}/../../Drivers/STM32H7xx_HAL_Driver/Src/stm32h7xx_hal_uart.c