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

vofa好了

Browse Source
This commit is contained in:
Robofish 2026-02-01 13:12:06 +08:00
parent b837b7f147
commit 796dbb2c16
20 changed files with 10642 additions and 9651 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
CMakeLists.txt
View File

@ -79,6 +79,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/device/motor_lz.c
User/device/motor_rm.c
User/device/vision_bridge.c
User/device/vofa.c
# User/module sources
User/module/balance_chassis.c

2
Core/Inc/stm32h7xx_it.h
View File

@ -56,6 +56,8 @@ void DMA1_Stream0_IRQHandler(void);
void DMA1_Stream1_IRQHandler(void);
void DMA1_Stream2_IRQHandler(void);
void DMA1_Stream3_IRQHandler(void);
void DMA1_Stream4_IRQHandler(void);
void DMA1_Stream5_IRQHandler(void);
void FDCAN1_IT0_IRQHandler(void);
void FDCAN2_IT0_IRQHandler(void);
void FDCAN1_IT1_IRQHandler(void);

6
Core/Src/dma.c
View File

@ -55,6 +55,12 @@ void MX_DMA_Init(void)
/* DMA1_Stream3_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream3_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream3_IRQn);
/* DMA1_Stream4_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream4_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream4_IRQn);
/* DMA1_Stream5_IRQn interrupt configuration */
HAL_NVIC_SetPriority(DMA1_Stream5_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(DMA1_Stream5_IRQn);
}

30
Core/Src/stm32h7xx_it.c
View File

@ -63,6 +63,8 @@ extern DMA_HandleTypeDef hdma_spi2_rx;
extern DMA_HandleTypeDef hdma_spi2_tx;
extern SPI_HandleTypeDef hspi2;
extern DMA_HandleTypeDef hdma_uart5_rx;
extern DMA_HandleTypeDef hdma_usart1_tx;
extern DMA_HandleTypeDef hdma_usart1_rx;
extern UART_HandleTypeDef huart5;
extern UART_HandleTypeDef huart7;
extern UART_HandleTypeDef huart1;
@ -229,6 +231,34 @@ void DMA1_Stream3_IRQHandler(void)
/* USER CODE END DMA1_Stream3_IRQn 1 */
}
/**
* @brief This function handles DMA1 stream4 global interrupt.
*/
void DMA1_Stream4_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Stream4_IRQn 0 */
/* USER CODE END DMA1_Stream4_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart1_tx);
/* USER CODE BEGIN DMA1_Stream4_IRQn 1 */
/* USER CODE END DMA1_Stream4_IRQn 1 */
}
/**
* @brief This function handles DMA1 stream5 global interrupt.
*/
void DMA1_Stream5_IRQHandler(void)
{
/* USER CODE BEGIN DMA1_Stream5_IRQn 0 */
/* USER CODE END DMA1_Stream5_IRQn 0 */
HAL_DMA_IRQHandler(&hdma_usart1_rx);
/* USER CODE BEGIN DMA1_Stream5_IRQn 1 */
/* USER CODE END DMA1_Stream5_IRQn 1 */
}
/**
* @brief This function handles FDCAN1 interrupt 0.
*/

43
Core/Src/usart.c
View File

@ -31,6 +31,8 @@ UART_HandleTypeDef huart2;
UART_HandleTypeDef huart3;
UART_HandleTypeDef huart10;
DMA_HandleTypeDef hdma_uart5_rx;
DMA_HandleTypeDef hdma_usart1_tx;
DMA_HandleTypeDef hdma_usart1_rx;
/* UART5 init function */
void MX_UART5_Init(void)
@ -431,6 +433,43 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
GPIO_InitStruct.Alternate = GPIO_AF7_USART1;
HAL_GPIO_Init(GPIOA, &GPIO_InitStruct);
/* USART1 DMA Init */
/* USART1_TX Init */
hdma_usart1_tx.Instance = DMA1_Stream4;
hdma_usart1_tx.Init.Request = DMA_REQUEST_USART1_TX;
hdma_usart1_tx.Init.Direction = DMA_MEMORY_TO_PERIPH;
hdma_usart1_tx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_usart1_tx.Init.MemInc = DMA_MINC_ENABLE;
hdma_usart1_tx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_usart1_tx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_usart1_tx.Init.Mode = DMA_NORMAL;
hdma_usart1_tx.Init.Priority = DMA_PRIORITY_LOW;
hdma_usart1_tx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_usart1_tx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(uartHandle,hdmatx,hdma_usart1_tx);
/* USART1_RX Init */
hdma_usart1_rx.Instance = DMA1_Stream5;
hdma_usart1_rx.Init.Request = DMA_REQUEST_USART1_RX;
hdma_usart1_rx.Init.Direction = DMA_PERIPH_TO_MEMORY;
hdma_usart1_rx.Init.PeriphInc = DMA_PINC_DISABLE;
hdma_usart1_rx.Init.MemInc = DMA_MINC_ENABLE;
hdma_usart1_rx.Init.PeriphDataAlignment = DMA_PDATAALIGN_BYTE;
hdma_usart1_rx.Init.MemDataAlignment = DMA_MDATAALIGN_BYTE;
hdma_usart1_rx.Init.Mode = DMA_NORMAL;
hdma_usart1_rx.Init.Priority = DMA_PRIORITY_LOW;
hdma_usart1_rx.Init.FIFOMode = DMA_FIFOMODE_DISABLE;
if (HAL_DMA_Init(&hdma_usart1_rx) != HAL_OK)
{
Error_Handler();
}
__HAL_LINKDMA(uartHandle,hdmarx,hdma_usart1_rx);
/* USART1 interrupt Init */
HAL_NVIC_SetPriority(USART1_IRQn, 5, 0);
HAL_NVIC_EnableIRQ(USART1_IRQn);
@ -630,6 +669,10 @@ void HAL_UART_MspDeInit(UART_HandleTypeDef* uartHandle)
*/
HAL_GPIO_DeInit(GPIOA, GPIO_PIN_9|GPIO_PIN_10);
/* USART1 DMA DeInit */
HAL_DMA_DeInit(uartHandle->hdmatx);
HAL_DMA_DeInit(uartHandle->hdmarx);
/* USART1 interrupt Deinit */
HAL_NVIC_DisableIRQ(USART1_IRQn);
/* USER CODE BEGIN USART1_MspDeInit 1 */

42
CtrBoard-H7_ALL.ioc
View File

@ -51,7 +51,9 @@ Dma.Request0=ADC1
Dma.Request1=SPI2_RX
Dma.Request2=SPI2_TX
Dma.Request3=UART5_RX
Dma.RequestsNb=4
Dma.Request4=USART1_TX
Dma.Request5=USART1_RX
Dma.RequestsNb=6
Dma.SPI2_RX.1.Direction=DMA_PERIPH_TO_MEMORY
Dma.SPI2_RX.1.EventEnable=DISABLE
Dma.SPI2_RX.1.FIFOMode=DMA_FIFOMODE_DISABLE
@ -106,6 +108,42 @@ Dma.UART5_RX.3.SyncEnable=DISABLE
Dma.UART5_RX.3.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
Dma.UART5_RX.3.SyncRequestNumber=1
Dma.UART5_RX.3.SyncSignalID=NONE
Dma.USART1_RX.5.Direction=DMA_PERIPH_TO_MEMORY
Dma.USART1_RX.5.EventEnable=DISABLE
Dma.USART1_RX.5.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.USART1_RX.5.Instance=DMA1_Stream5
Dma.USART1_RX.5.MemDataAlignment=DMA_MDATAALIGN_BYTE
Dma.USART1_RX.5.MemInc=DMA_MINC_ENABLE
Dma.USART1_RX.5.Mode=DMA_NORMAL
Dma.USART1_RX.5.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.USART1_RX.5.PeriphInc=DMA_PINC_DISABLE
Dma.USART1_RX.5.Polarity=HAL_DMAMUX_REQ_GEN_RISING
Dma.USART1_RX.5.Priority=DMA_PRIORITY_LOW
Dma.USART1_RX.5.RequestNumber=1
Dma.USART1_RX.5.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
Dma.USART1_RX.5.SignalID=NONE
Dma.USART1_RX.5.SyncEnable=DISABLE
Dma.USART1_RX.5.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
Dma.USART1_RX.5.SyncRequestNumber=1
Dma.USART1_RX.5.SyncSignalID=NONE
Dma.USART1_TX.4.Direction=DMA_MEMORY_TO_PERIPH
Dma.USART1_TX.4.EventEnable=DISABLE
Dma.USART1_TX.4.FIFOMode=DMA_FIFOMODE_DISABLE
Dma.USART1_TX.4.Instance=DMA1_Stream4
Dma.USART1_TX.4.MemDataAlignment=DMA_MDATAALIGN_BYTE
Dma.USART1_TX.4.MemInc=DMA_MINC_ENABLE
Dma.USART1_TX.4.Mode=DMA_NORMAL
Dma.USART1_TX.4.PeriphDataAlignment=DMA_PDATAALIGN_BYTE
Dma.USART1_TX.4.PeriphInc=DMA_PINC_DISABLE
Dma.USART1_TX.4.Polarity=HAL_DMAMUX_REQ_GEN_RISING
Dma.USART1_TX.4.Priority=DMA_PRIORITY_LOW
Dma.USART1_TX.4.RequestNumber=1
Dma.USART1_TX.4.RequestParameters=Instance,Direction,PeriphInc,MemInc,PeriphDataAlignment,MemDataAlignment,Mode,Priority,FIFOMode,SignalID,Polarity,RequestNumber,SyncSignalID,SyncPolarity,SyncEnable,EventEnable,SyncRequestNumber
Dma.USART1_TX.4.SignalID=NONE
Dma.USART1_TX.4.SyncEnable=DISABLE
Dma.USART1_TX.4.SyncPolarity=HAL_DMAMUX_SYNC_NO_EVENT
Dma.USART1_TX.4.SyncRequestNumber=1
Dma.USART1_TX.4.SyncSignalID=NONE
FDCAN1.AutoRetransmission=ENABLE
FDCAN1.CalculateBaudRateNominal=1000000
FDCAN1.CalculateTimeBitNominal=1000
@ -256,6 +294,8 @@ NVIC.DMA1_Stream0_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream1_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream2_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream3_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_Stream4_IRQn=true\:5\:0\:false\:false\:true\:true\:false\:true\:true
NVIC.DMA1_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

54
MDK-ARM/CtrBoard-H7_ALL.uvoptx
View File

@ -1323,6 +1323,18 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>8</GroupNumber>
<FileNumber>85</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\device\vofa.c</PathWithFileName>
<FilenameWithoutPath>vofa.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>
@ -1333,7 +1345,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>85</FileNumber>
<FileNumber>86</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1345,7 +1357,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>86</FileNumber>
<FileNumber>87</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1357,7 +1369,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>87</FileNumber>
<FileNumber>88</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1369,7 +1381,7 @@
</File>
<File>
<GroupNumber>9</GroupNumber>
<FileNumber>88</FileNumber>
<FileNumber>89</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1389,7 +1401,7 @@
<RteFlg>0</RteFlg>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>89</FileNumber>
<FileNumber>90</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1401,7 +1413,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>90</FileNumber>
<FileNumber>91</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1413,7 +1425,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>91</FileNumber>
<FileNumber>92</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1425,7 +1437,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>92</FileNumber>
<FileNumber>93</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1437,7 +1449,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>93</FileNumber>
<FileNumber>94</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1449,7 +1461,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>94</FileNumber>
<FileNumber>95</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1461,7 +1473,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>95</FileNumber>
<FileNumber>96</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1473,7 +1485,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>96</FileNumber>
<FileNumber>97</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1485,7 +1497,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>97</FileNumber>
<FileNumber>98</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1497,7 +1509,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>98</FileNumber>
<FileNumber>99</FileNumber>
<FileType>5</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1509,7 +1521,7 @@
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>99</FileNumber>
<FileNumber>100</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
@ -1519,6 +1531,18 @@
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
<File>
<GroupNumber>10</GroupNumber>
<FileNumber>101</FileNumber>
<FileType>1</FileType>
<tvExp>0</tvExp>
<tvExpOptDlg>0</tvExpOptDlg>
<bDave2>0</bDave2>
<PathWithFileName>..\User\task\vofa.c</PathWithFileName>
<FilenameWithoutPath>vofa.c</FilenameWithoutPath>
<RteFlg>0</RteFlg>
<bShared>0</bShared>
</File>
</Group>
<Group>

10
MDK-ARM/CtrBoard-H7_ALL.uvprojx
View File

@ -839,6 +839,11 @@
<FileType>1</FileType>
<FilePath>..\User\device\vision_bridge.c</FilePath>
</File>
<File>
<FileName>vofa.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\device\vofa.c</FilePath>
</File>
</Files>
</Group>
<Group>
@ -924,6 +929,11 @@
<FileType>1</FileType>
<FilePath>..\User\task\ai.c</FilePath>
</File>
<File>
<FileName>vofa.c</FileName>
<FileType>1</FileType>
<FilePath>..\User\task\vofa.c</FilePath>
</File>
</Files>
</Group>
<Group>

BIN
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.axf
View File

Binary file not shown.

19883
MDK-ARM/CtrBoard-H7_ALL/CtrBoard-H7_ALL.hex
View File

File diff suppressed because it is too large Load Diff

8
User/component/vmc.c
View File

@ -61,6 +61,9 @@ int8_t VMC_Init(VMC_t *vmc, const VMC_Param_t *param, float sample_freq) {
// 重置状态
VMC_Reset(vmc);
// 初始化dd_theta低通滤波器 (截止频率50Hz)
LowPassFilter2p_Init(&vmc->dd_theta_filter, sample_freq, 50.0f);
vmc->initialized = true;
return 0;
@ -124,8 +127,9 @@ int8_t VMC_ForwardSolve(VMC_t *vmc, float phi1, float phi4, float body_pitch, fl
leg->d_theta = body_pitch_rate + leg->d_phi0;
leg->d_L0 = VMC_ComputeNumericDerivative(leg->L0, leg->last_L0, vmc->dt);
// 计算角加速度
leg->dd_theta = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
// 计算角加速度并滤波
float dd_theta_raw = VMC_ComputeNumericDerivative(leg->d_theta, leg->last_d_theta, vmc->dt);
leg->dd_theta = LowPassFilter2p_Apply(&vmc->dd_theta_filter, dd_theta_raw);
VMC_GroundContactDetection(vmc); // 更新地面接触状态
return 0;

2
User/component/vmc.h
View File

@ -8,6 +8,7 @@ extern "C" {
#include <stdint.h>
#include <stdbool.h>
#include <math.h>
#include "filter.h"
/* Exported types ----------------------------------------------------------- */
/**
@ -83,6 +84,7 @@ typedef struct {
VMC_Leg_t leg; // 腿部状态
float dt; // 控制周期
bool initialized; // 初始化标志
LowPassFilter2p_t dd_theta_filter; // dd_theta低通滤波器
} VMC_t;
/* Exported constants ------------------------------------------------------- */

4
User/device/device_config.yaml
View File

@ -29,3 +29,7 @@ motor_lz:
motor_rm:
bsp_config: {}
enabled: true
vofa:
bsp_config:
BSP_UART_VOFA: BSP_UART_VOFA
enabled: true

106
User/device/vofa.c Normal file
View File

@ -0,0 +1,106 @@
/* Includes ----------------------------------------------------------------- */
#include <stdio.h>
#include <string.h>
#include "device/vofa.h"
#include "bsp/uart.h"
/* USER INCLUDE BEGIN */
/* USER INCLUDE END */
/* Private define ----------------------------------------------------------- */
#define MAX_CHANNEL 64u // 根据实际最大通道数调整
#define JUSTFLOAT_TAIL 0x7F800000
/* USER DEFINE BEGIN */
/* USER DEFINE END */
/* Private macro ------------------------------------------------------------ */
/* Private typedef ---------------------------------------------------------- */
/* USER STRUCT BEGIN */
/* USER STRUCT END */
/* Private variables -------------------------------------------------------- */
static uint8_t vofa_tx_buf[sizeof(float) * MAX_CHANNEL + sizeof(uint32_t)];
static VOFA_Protocol_t current_protocol = VOFA_PROTOCOL_FIREWATER; // 默认协议
/* Private function -------------------------------------------------------- */
/* USER FUNCTION BEGIN */
/* USER FUNCTION END */
/************************ RawData *************************/
void VOFA_RawData_Send(const char* data, bool dma) {
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t*)data, strlen(data), dma);
}
/************************ FireWater *************************/
void VOFA_FireWater_Send(float *channels, uint8_t channel_count, bool dma)
{
if (channel_count == 0 || channel_count > MAX_CHANNEL)
return;
char *buf = (char *)vofa_tx_buf;
size_t len = 0;
for (uint8_t i = 0; i < channel_count; ++i) {
len += snprintf(buf + len,
sizeof(vofa_tx_buf) - len,
"%s%.2f",
(i ? "," : ""),
channels[i]);
}
snprintf(buf + len, sizeof(vofa_tx_buf) - len, "\n");
BSP_UART_Transmit(BSP_UART_VOFA, vofa_tx_buf, strlen(buf), dma);
}
/************************ JustFloat *************************/
void VOFA_JustFloat_Send(float *channels, uint8_t channel_count, bool dma)
{
if (channel_count == 0 || channel_count > MAX_CHANNEL)
return;
memcpy(vofa_tx_buf, channels, channel_count * sizeof(float));
uint32_t tail = JUSTFLOAT_TAIL; // 0x7F800000
memcpy(vofa_tx_buf + channel_count * sizeof(float), &tail, sizeof(tail));
BSP_UART_Transmit(BSP_UART_VOFA, vofa_tx_buf, channel_count * sizeof(float) + sizeof(tail), dma);
}
/* Exported functions ------------------------------------------------------- */
int8_t VOFA_init(VOFA_Protocol_t protocol) {
current_protocol = protocol;
return DEVICE_OK;
}
int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma) {
switch (current_protocol) {
case VOFA_PROTOCOL_RAWDATA:
{
char data[256];
if (channel_count >= 1) {
sprintf(data, "Channel1: %.2f", channels[0]);
if (channel_count >= 2) {
sprintf(data + strlen(data), ", Channel2: %.2f", channels[1]);
}
strcat(data, "\n");
VOFA_RawData_Send(data, dma);
}
}
break;
case VOFA_PROTOCOL_FIREWATER:
VOFA_FireWater_Send(channels, channel_count, dma);
break;
case VOFA_PROTOCOL_JUSTFLOAT:
VOFA_JustFloat_Send(channels, channel_count, dma);
break;
default:
return DEVICE_ERR;
}
return DEVICE_OK;
}

39
User/device/vofa.h Normal file
View File

@ -0,0 +1,39 @@
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include "bsp/uart.h"
#include "device/device.h"
/* Exported constants ------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
/* Exported types ----------------------------------------------------------- */
/* Exported functions prototypes -------------------------------------------- */
typedef enum {
VOFA_PROTOCOL_RAWDATA,
VOFA_PROTOCOL_FIREWATER,
VOFA_PROTOCOL_JUSTFLOAT,
} VOFA_Protocol_t;
/**
* @brief 初始化VOFA设备
* @param protocol 设置通信协议
* @return
*/
int8_t VOFA_init(VOFA_Protocol_t protocol);
/**
* @brief 发送数据到VOFA
* @param channels 要发送的通道数据
* @param channel_count 通道数量
* @param dma 是否使用DMA发送
* @return
*/
int8_t VOFA_Send(float* channels, uint8_t channel_count, bool dma);
#ifdef __cplusplus
}
#endif

2
User/task/ctrl_chassis.c
View File

@ -69,6 +69,8 @@ void Task_ctrl_chassis(void *argument) {
}
Chassis_Control(&chassis, &chassis_cmd);
osMessageQueueReset(task_runtime.msgq.chassis.vofa); // 重置消息队列，防止阻塞
osMessageQueuePut(task_runtime.msgq.chassis.vofa, &chassis, 0, 0);
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}

2
User/task/init.c
View File

@ -49,8 +49,10 @@ void Task_Init(void *argument) {
task_runtime.msgq.chassis.imu = osMessageQueueNew(2u, sizeof(Chassis_IMU_t), NULL);
task_runtime.msgq.chassis.cmd = osMessageQueueNew(2u, sizeof(Chassis_CMD_t), NULL);
task_runtime.msgq.chassis.yaw = osMessageQueueNew(2u, sizeof(MOTOR_Feedback_t), NULL);
task_runtime.msgq.chassis.vofa = osMessageQueueNew(2u, sizeof(Chassis_t), NULL);
task_runtime.msgq.gimbal.cmd = osMessageQueueNew(2u, sizeof(Gimbal_CMD_t), NULL);
task_runtime.msgq.gimbal.ai_cmd = osMessageQueueNew(2u, sizeof(Gimbal_AI_t), NULL);
/* USER MESSAGE END */
osKernelUnlock(); // 解锁内核

8
User/task/rc.c
View File

@ -132,13 +132,13 @@ switch (dr16.data.sw_l) {
cmd_for_chassis.mode = CHASSIS_MODE_RELAX;
break;
case DR16_SW_MID:
cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
break;
case DR16_SW_DOWN:
// cmd_for_chassis.mode = CHASSIS_MODE_RECOVER;
// cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
cmd_for_chassis.mode = CHASSIS_MODE_BALANCE_ROTOR;
// cmd_for_chassis.mode = CHASSIS_MODE_WHELL_LEG_BALANCE;
// cmd_for_chassis.mode = CHASSIS_MODE_CALIBRATE;
break;
default:

1
User/task/user_task.h
View File

@ -60,6 +60,7 @@ typedef struct {
osMessageQueueId_t imu;
osMessageQueueId_t cmd;
osMessageQueueId_t yaw;
osMessageQueueId_t vofa;
}chassis;
struct {
osMessageQueueId_t imu;

46
User/task/vofa.c
View File

@ -6,7 +6,8 @@
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "module/balance_chassis.h"
#include "device/vofa.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
@ -14,7 +15,7 @@
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
Chassis_t chassis_vofa;
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
@ -30,13 +31,52 @@ void Task_vofa(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
VOFA_init(VOFA_PROTOCOL_JUSTFLOAT);
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
// 从消息队列获取chassis数据
if (osMessageQueueGet(task_runtime.msgq.chassis.vofa, &chassis_vofa, NULL, 0) == osOK) {
// 准备要发送的数据通道
float channels[16];
// 通道0-1: 左右腿的VMC腿长
channels[0] = chassis_vofa.vmc_[0].leg.L0;
channels[1] = chassis_vofa.vmc_[1].leg.L0;
// 通道2-3: 左右腿的VMC角度
channels[2] = chassis_vofa.vmc_[0].leg.theta;
channels[3] = chassis_vofa.vmc_[1].leg.theta;
// 通道4-6: IMU欧拉角 (pitch, roll, yaw)
channels[4] = chassis_vofa.feedback.imu.euler.pit;
channels[5] = chassis_vofa.feedback.imu.euler.rol;
channels[6] = chassis_vofa.feedback.imu.euler.yaw;
// 通道7-8: 左右轮速度
channels[7] = chassis_vofa.vmc_[0].leg.d_theta;
channels[8] = chassis_vofa.vmc_[0].leg.dd_theta;
// 通道9-10: 机体位置和速度
channels[9] = chassis_vofa.chassis_state.position_x;
channels[10] = chassis_vofa.chassis_state.velocity_x;
// 通道11-12: 左右腿的虚拟支撑力
channels[11] = chassis_vofa.vmc_[0].leg.F_virtual;
channels[12] = chassis_vofa.vmc_[1].leg.F_virtual;
// 通道13-14: 左右腿的虚拟摆力矩
channels[13] = chassis_vofa.vmc_[0].leg.T_virtual;
channels[14] = chassis_vofa.vmc_[1].leg.T_virtual;
// 通道15: yaw控制力矩
channels[15] = chassis_vofa.yaw_control.yaw_force;
// 使用JustFloat协议和DMA发送
VOFA_Send(channels, 16, true);
}
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}