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 2025-10-04 17:38:10 +08:00
parent 7d868bf32a
commit ff2a3c5862
15 changed files with 412 additions and 241 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
.mxproject
View File

File diff suppressed because one or more lines are too long

4
CMakeLists.txt
View File

@ -60,8 +60,10 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/device/motor_lk.c User/device/motor_lk.c
User/device/motor_lz.c User/device/motor_lz.c
# User/module sources
User/module/config.c
# User/task sources # User/task sources
User/task/Task4.c
User/task/blink.c User/task/blink.c
User/task/ctrl_lz.c User/task/ctrl_lz.c
User/task/imu.c User/task/imu.c

4
Core/Src/can.c
View File

@ -47,7 +47,7 @@ void MX_CAN1_Init(void)
hcan1.Init.TimeTriggeredMode = DISABLE; hcan1.Init.TimeTriggeredMode = DISABLE;
hcan1.Init.AutoBusOff = DISABLE; hcan1.Init.AutoBusOff = DISABLE;
hcan1.Init.AutoWakeUp = DISABLE; hcan1.Init.AutoWakeUp = DISABLE;
hcan1.Init.AutoRetransmission = DISABLE; hcan1.Init.AutoRetransmission = ENABLE;
hcan1.Init.ReceiveFifoLocked = DISABLE; hcan1.Init.ReceiveFifoLocked = DISABLE;
hcan1.Init.TransmitFifoPriority = ENABLE; hcan1.Init.TransmitFifoPriority = ENABLE;
if (HAL_CAN_Init(&hcan1) != HAL_OK) if (HAL_CAN_Init(&hcan1) != HAL_OK)
@ -79,7 +79,7 @@ void MX_CAN2_Init(void)
hcan2.Init.TimeTriggeredMode = DISABLE; hcan2.Init.TimeTriggeredMode = DISABLE;
hcan2.Init.AutoBusOff = DISABLE; hcan2.Init.AutoBusOff = DISABLE;
hcan2.Init.AutoWakeUp = DISABLE; hcan2.Init.AutoWakeUp = DISABLE;
hcan2.Init.AutoRetransmission = DISABLE; hcan2.Init.AutoRetransmission = ENABLE;
hcan2.Init.ReceiveFifoLocked = DISABLE; hcan2.Init.ReceiveFifoLocked = DISABLE;
hcan2.Init.TransmitFifoPriority = ENABLE; hcan2.Init.TransmitFifoPriority = ENABLE;
if (HAL_CAN_Init(&hcan2) != HAL_OK) if (HAL_CAN_Init(&hcan2) != HAL_OK)

318
Core/Src/freertos.c
View File

@ -1,159 +1,159 @@
/* USER CODE BEGIN Header */ /* USER CODE BEGIN Header */
/** /**
****************************************************************************** ******************************************************************************
* File Name : freertos.c * File Name : freertos.c
* Description : Code for freertos applications * Description : Code for freertos applications
****************************************************************************** ******************************************************************************
* @attention * @attention
* *
* <h2><center>&copy; Copyright (c) 2025 STMicroelectronics. * <h2><center>&copy; Copyright (c) 2025 STMicroelectronics.
* All rights reserved.</center></h2> * All rights reserved.</center></h2>
* *
* This software component is licensed by ST under Ultimate Liberty license * This software component is licensed by ST under Ultimate Liberty license
* SLA0044, the "License"; You may not use this file except in compliance with * SLA0044, the "License"; You may not use this file except in compliance with
* the License. You may obtain a copy of the License at: * the License. You may obtain a copy of the License at:
* www.st.com/SLA0044 * www.st.com/SLA0044
* *
****************************************************************************** ******************************************************************************
*/ */
/* USER CODE END Header */ /* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/ /* Includes ------------------------------------------------------------------*/
#include "FreeRTOS.h" #include "FreeRTOS.h"
#include "task.h" #include "task.h"
#include "main.h" #include "main.h"
#include "cmsis_os.h" #include "cmsis_os.h"
/* Private includes ----------------------------------------------------------*/ /* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */ /* USER CODE BEGIN Includes */
#include "task/user_task.h" #include "task/user_task.h"
/* USER CODE END Includes */ /* USER CODE END Includes */
/* Private typedef -----------------------------------------------------------*/ /* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */ /* USER CODE BEGIN PTD */
/* USER CODE END PTD */ /* USER CODE END PTD */
/* Private define ------------------------------------------------------------*/ /* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */ /* USER CODE BEGIN PD */
/* USER CODE END PD */ /* USER CODE END PD */
/* Private macro -------------------------------------------------------------*/ /* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */ /* USER CODE BEGIN PM */
/* USER CODE END PM */ /* USER CODE END PM */
/* Private variables ---------------------------------------------------------*/ /* Private variables ---------------------------------------------------------*/
/* USER CODE BEGIN Variables */ /* USER CODE BEGIN Variables */
/* USER CODE END Variables */ /* USER CODE END Variables */
/* Definitions for defaultTask */ /* Definitions for defaultTask */
osThreadId_t defaultTaskHandle; osThreadId_t defaultTaskHandle;
const osThreadAttr_t defaultTask_attributes = { const osThreadAttr_t defaultTask_attributes = {
.name = "defaultTask", .name = "defaultTask",
.stack_size = 128 * 4, .stack_size = 128 * 4,
.priority = (osPriority_t) osPriorityNormal, .priority = (osPriority_t) osPriorityNormal,
}; };
/* Private function prototypes -----------------------------------------------*/ /* Private function prototypes -----------------------------------------------*/
/* USER CODE BEGIN FunctionPrototypes */ /* USER CODE BEGIN FunctionPrototypes */
/* USER CODE END FunctionPrototypes */ /* USER CODE END FunctionPrototypes */
void StartDefaultTask(void *argument); void StartDefaultTask(void *argument);
extern void MX_USB_DEVICE_Init(void); extern void MX_USB_DEVICE_Init(void);
void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */ void MX_FREERTOS_Init(void); /* (MISRA C 2004 rule 8.1) */
/* Hook prototypes */ /* Hook prototypes */
void configureTimerForRunTimeStats(void); void configureTimerForRunTimeStats(void);
unsigned long getRunTimeCounterValue(void); unsigned long getRunTimeCounterValue(void);
void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName); void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName);
/* USER CODE BEGIN 1 */ /* USER CODE BEGIN 1 */
/* Functions needed when configGENERATE_RUN_TIME_STATS is on */ /* Functions needed when configGENERATE_RUN_TIME_STATS is on */
__weak void configureTimerForRunTimeStats(void) __weak void configureTimerForRunTimeStats(void)
{ {
} }
__weak unsigned long getRunTimeCounterValue(void) __weak unsigned long getRunTimeCounterValue(void)
{ {
return 0; return 0;
} }
/* USER CODE END 1 */ /* USER CODE END 1 */
/* USER CODE BEGIN 4 */ /* USER CODE BEGIN 4 */
void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName) void vApplicationStackOverflowHook(TaskHandle_t xTask, signed char *pcTaskName)
{ {
/* Run time stack overflow checking is performed if /* Run time stack overflow checking is performed if
configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is configCHECK_FOR_STACK_OVERFLOW is defined to 1 or 2. This hook function is
called if a stack overflow is detected. */ called if a stack overflow is detected. */
} }
/* USER CODE END 4 */ /* USER CODE END 4 */
/** /**
* @brief FreeRTOS initialization * @brief FreeRTOS initialization
* @param None * @param None
* @retval None * @retval None
*/ */
void MX_FREERTOS_Init(void) { void MX_FREERTOS_Init(void) {
/* USER CODE BEGIN Init */ /* USER CODE BEGIN Init */
/* USER CODE END Init */ /* USER CODE END Init */
/* USER CODE BEGIN RTOS_MUTEX */ /* USER CODE BEGIN RTOS_MUTEX */
/* add mutexes, ... */ /* add mutexes, ... */
/* USER CODE END RTOS_MUTEX */ /* USER CODE END RTOS_MUTEX */
/* USER CODE BEGIN RTOS_SEMAPHORES */ /* USER CODE BEGIN RTOS_SEMAPHORES */
/* add semaphores, ... */ /* add semaphores, ... */
/* USER CODE END RTOS_SEMAPHORES */ /* USER CODE END RTOS_SEMAPHORES */
/* USER CODE BEGIN RTOS_TIMERS */ /* USER CODE BEGIN RTOS_TIMERS */
/* start timers, add new ones, ... */ /* start timers, add new ones, ... */
/* USER CODE END RTOS_TIMERS */ /* USER CODE END RTOS_TIMERS */
/* USER CODE BEGIN RTOS_QUEUES */ /* USER CODE BEGIN RTOS_QUEUES */
/* add queues, ... */ /* add queues, ... */
/* USER CODE END RTOS_QUEUES */ /* USER CODE END RTOS_QUEUES */
/* Create the thread(s) */ /* Create the thread(s) */
/* creation of defaultTask */ /* creation of defaultTask */
defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes); defaultTaskHandle = osThreadNew(StartDefaultTask, NULL, &defaultTask_attributes);
/* USER CODE BEGIN RTOS_THREADS */ /* USER CODE BEGIN RTOS_THREADS */
/* add threads, ... */ /* add threads, ... */
osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务 osThreadNew(Task_Init, NULL, &attr_init); // 创建初始化任务
/* USER CODE END RTOS_THREADS */ /* USER CODE END RTOS_THREADS */
/* USER CODE BEGIN RTOS_EVENTS */ /* USER CODE BEGIN RTOS_EVENTS */
/* add events, ... */ /* add events, ... */
/* USER CODE END RTOS_EVENTS */ /* USER CODE END RTOS_EVENTS */
} }
/* USER CODE BEGIN Header_StartDefaultTask */ /* USER CODE BEGIN Header_StartDefaultTask */
/** /**
* @brief Function implementing the defaultTask thread. * @brief Function implementing the defaultTask thread.
* @param argument: Not used * @param argument: Not used
* @retval None * @retval None
*/ */
/* USER CODE END Header_StartDefaultTask */ /* USER CODE END Header_StartDefaultTask */
void StartDefaultTask(void *argument) void StartDefaultTask(void *argument)
{ {
/* init code for USB_DEVICE */ /* init code for USB_DEVICE */
MX_USB_DEVICE_Init(); MX_USB_DEVICE_Init();
/* USER CODE BEGIN StartDefaultTask */ /* USER CODE BEGIN StartDefaultTask */
osThreadTerminate(osThreadGetId()); osThreadTerminate(osThreadGetId());
/* USER CODE END StartDefaultTask */ /* USER CODE END StartDefaultTask */
} }
/* Private application code --------------------------------------------------*/ /* Private application code --------------------------------------------------*/
/* USER CODE BEGIN Application */ /* USER CODE BEGIN Application */
/* USER CODE END Application */ /* USER CODE END Application */
/************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/ /************************ (C) COPYRIGHT STMicroelectronics *****END OF FILE****/

6
DevC.ioc
View File

@ -21,7 +21,8 @@ CAN1.BS2=CAN_BS2_7TQ
CAN1.CalculateBaudRate=1000000 CAN1.CalculateBaudRate=1000000
CAN1.CalculateTimeBit=1000 CAN1.CalculateTimeBit=1000
CAN1.CalculateTimeQuantum=71.42857142857143 CAN1.CalculateTimeQuantum=71.42857142857143
CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate CAN1.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,ABOM,CalculateTimeBit,CalculateBaudRate,NART
CAN1.NART=ENABLE
CAN1.Prescaler=3 CAN1.Prescaler=3
CAN1.TXFP=ENABLE CAN1.TXFP=ENABLE
CAN2.BS1=CAN_BS1_6TQ CAN2.BS1=CAN_BS1_6TQ
@ -29,7 +30,8 @@ CAN2.BS2=CAN_BS2_7TQ
CAN2.CalculateBaudRate=1000000 CAN2.CalculateBaudRate=1000000
CAN2.CalculateTimeBit=1000 CAN2.CalculateTimeBit=1000
CAN2.CalculateTimeQuantum=71.42857142857143 CAN2.CalculateTimeQuantum=71.42857142857143
CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate CAN2.IPParameters=CalculateTimeQuantum,BS1,BS2,Prescaler,TXFP,CalculateTimeBit,CalculateBaudRate,NART
CAN2.NART=ENABLE
CAN2.Prescaler=3 CAN2.Prescaler=3
CAN2.TXFP=ENABLE CAN2.TXFP=ENABLE
Dma.I2C2_TX.2.Direction=DMA_MEMORY_TO_PERIPH Dma.I2C2_TX.2.Direction=DMA_MEMORY_TO_PERIPH

3
User/device/dm_imu.c
View File

@ -252,8 +252,9 @@ int8_t DM_IMU_AutoUpdateAll(DM_IMU_t *imu){
count++; count++;
if (count >= 4) { if (count >= 4) {
count = 0; // 重置计数器 count = 0; // 重置计数器
return DEVICE_OK;
} }
return DEVICE_OK; return DEVICE_ERR; // 未完成一轮更新
} }
/** /**

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

@ -0,0 +1,74 @@
/*
* 配置相关
*/
/* Includes ----------------------------------------------------------------- */
#include "module/config.h"
#include "bsp/can.h"
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* Exported variables ------------------------------------------------------- */
// 机器人参数配置
Config_RobotParam_t robot_config = {
.imu_param =
{
.can = BSP_CAN_2,
.can_id = 0x6FF,
.device_id = 0x42,
.master_id = 0x43,
},
.joint_param =
{
{
// 左髋关节
.can = BSP_CAN_2,
.motor_id = 124,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
{
// 左膝关节
.can = BSP_CAN_2,
.motor_id = 125,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = false,
.mode = MOTOR_LZ_MODE_MOTION,
},
{
// 右膝关节
.can = BSP_CAN_2,
.motor_id = 126,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = true,
.mode = MOTOR_LZ_MODE_MOTION,
},
{
// 右髋关节
.can = BSP_CAN_2,
.motor_id = 127,
.host_id = 0xFF,
.module = MOTOR_LZ_RSO3,
.reverse = true,
.mode = MOTOR_LZ_MODE_MOTION,
},
},
};
/* Private function prototypes ---------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
/**
* @brief 获取机器人配置参数
* @return 机器人配置参数指针
*/
Config_RobotParam_t *Config_GetRobotParam(void) { return &robot_config; }

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

@ -0,0 +1,31 @@
/*
* 配置相关
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
#include <stdint.h>
#include "device/dm_imu.h"
#include "device/motor_lz.h"
#include "device/motor_lk.h"
typedef struct {
DM_IMU_Param_t imu_param;
MOTOR_LZ_Param_t joint_param[4];
MOTOR_LK_Param_t wheel_param[2];
} Config_RobotParam_t;
/* Exported functions prototypes -------------------------------------------- */
/**
* @brief 获取机器人配置参数
* @return 机器人配置参数指针
*/
Config_RobotParam_t* Config_GetRobotParam(void);
#ifdef __cplusplus
}
#endif

44
User/task/Task4.c
View File

@ -1,44 +0,0 @@
/*
Task4 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_Task4(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
/* 计算任务运行到指定频率需要等待的tick数 */
const uint32_t delay_tick = osKernelGetTickFreq() / TASK4_FREQ;
osDelay(TASK4_INIT_DELAY); /* 延时一段时间再开启任务 */
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */
/* USER CODE INIT END */
while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */
/* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
}
}

7
User/task/config.yaml
View File

@ -19,10 +19,3 @@
function: Task_ctrl_lz function: Task_ctrl_lz
name: ctrl_lz name: ctrl_lz
stack: 256 stack: 256
- delay: 0
description: ''
freq_control: true
frequency: 500.0
function: Task_Task4
name: Task4
stack: 256

90
User/task/ctrl_lz.c
View File

@ -6,7 +6,11 @@
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "bsp/can.h"
#include "device/motor_lz.h"
#include "device/motor_lk.h"
#include "module/config.h"
#include <string.h>
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -14,7 +18,7 @@
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
static bool command_received = false;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -30,12 +34,94 @@ void Task_ctrl_lz(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
BSP_CAN_Init();
MOTOR_LZ_Init();
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Register(&Config_GetRobotParam()->joint_param[i]);
}
// 注册CAN接收ID
BSP_CAN_RegisterId(BSP_CAN_1, 121, 0); // 使能命令
BSP_CAN_RegisterId(BSP_CAN_1, 122, 0); // 力矩控制命令
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
MOTOR_LZ_UpdateAll();
// 检查CAN接收消息
BSP_CAN_Message_t rx_msg;
command_received = false; // 重置命令接收标志
// 检查ID 121 - 使能4个电机
if (BSP_CAN_GetMessage(BSP_CAN_1, 121, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
command_received = true;
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Enable(&Config_GetRobotParam()->joint_param[i]);
}
}
// 检查ID 122 - 运控模式控制4个电机
if (BSP_CAN_GetMessage(BSP_CAN_1, 122, &rx_msg, BSP_CAN_TIMEOUT_IMMEDIATE) == BSP_OK) {
command_received = true;
// 8字节数据分别是4个电机的力矩 (每个电机2字节，有符号整数，精度0.01 Nm)
for (int i = 0; i < 4; i++) {
int16_t torque_raw;
memcpy(&torque_raw, &rx_msg.data[i * 2], sizeof(int16_t));
float torque = (float)torque_raw / 100.0f; // 转换为浮点数力矩值
// 使用运控模式控制电机，只设置力矩，其他参数为0
MOTOR_LZ_MotionParam_t motion_param = {
.target_angle = 0.0f,
.target_velocity = 0.0f,
.kp = 0.0f,
.kd = 0.0f,
.torque = torque
};
MOTOR_LZ_MotionControl(&Config_GetRobotParam()->joint_param[i], &motion_param);
}
}
// 如果没有收到任何控制命令，电机进入relax模式
if (!command_received) {
for (int i = 0; i < 4; i++) {
MOTOR_LZ_Relax(&Config_GetRobotParam()->joint_param[i]);
}
}
// 发送4个电机的反馈数据，ID分别为124、125、126、127
for (int i = 0; i < 4; i++) {
MOTOR_LZ_t* motor = MOTOR_LZ_GetMotor(&Config_GetRobotParam()->joint_param[i]);
if (motor != NULL) {
BSP_CAN_StdDataFrame_t motor_frame = {
.id = 124 + i, // ID: 124, 125, 126, 127
.dlc = 8,
.data = {0}
};
// 数据重构：转矩电流(2字节) + 位置(3字节) + 速度(3字节) = 8字节
// 转矩电流 - 转换为16位整数 (精度0.01 Nm)
int16_t torque_int = (int16_t)(motor->lz_feedback.current_torque * 100);
memcpy(&motor_frame.data[0], &torque_int, sizeof(int16_t));
// 位置 - 转换为24位整数，使用3字节 (精度0.0001 rad)
int32_t angle_int = (int32_t)(motor->lz_feedback.current_angle * 10000) & 0xFFFFFF;
motor_frame.data[2] = (angle_int >> 16) & 0xFF;
motor_frame.data[3] = (angle_int >> 8) & 0xFF;
motor_frame.data[4] = angle_int & 0xFF;
// 速度 - 转换为24位整数，使用3字节 (精度0.001 rad/s)
int32_t velocity_int = (int32_t)(motor->lz_feedback.current_velocity * 1000) & 0xFFFFFF;
motor_frame.data[5] = (velocity_int >> 16) & 0xFF;
motor_frame.data[6] = (velocity_int >> 8) & 0xFF;
motor_frame.data[7] = velocity_int & 0xFF;
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &motor_frame);
}
}
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */

48
User/task/imu.c
View File

@ -6,7 +6,10 @@
/* Includes ----------------------------------------------------------------- */ /* Includes ----------------------------------------------------------------- */
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "bsp/can.h"
#include "device/dm_imu.h"
#include "module/config.h"
#include <string.h>
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -14,7 +17,7 @@
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* USER STRUCT BEGIN */
DM_IMU_t dm_imu;
/* USER STRUCT END */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
@ -30,13 +33,50 @@ void Task_imu(void *argument) {
uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */ uint32_t tick = osKernelGetTickCount(); /* 控制任务运行频率的计时 */
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
BSP_CAN_Init();
DM_IMU_Init(&dm_imu, &Config_GetRobotParam()->imu_param);
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {
tick += delay_tick; /* 计算下一个唤醒时刻 */ tick += delay_tick; /* 计算下一个唤醒时刻 */
/* USER CODE BEGIN */ /* USER CODE BEGIN */
if (DM_IMU_AutoUpdateAll(&dm_imu) == DEVICE_OK) {
// 发送加速度计数据 (ID: 0x66) - 前8字节 (x, y)
BSP_CAN_StdDataFrame_t accl_frame = {
.id = 0x66,
.dlc = 8,
.data = {0}
};
memcpy(accl_frame.data, &dm_imu.data.accl, 8);
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &accl_frame);
// 发送陀螺仪数据 (ID: 0x67) - 前8字节 (x, y)
BSP_CAN_StdDataFrame_t gyro_frame = {
.id = 0x67,
.dlc = 8,
.data = {0}
};
memcpy(gyro_frame.data, &dm_imu.data.gyro, 8);
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &gyro_frame);
// 发送欧拉角数据 (ID: 0x68) - 前8字节 (yaw, pit)
BSP_CAN_StdDataFrame_t euler_frame = {
.id = 0x68,
.dlc = 8,
.data = {0}
};
memcpy(euler_frame.data, &dm_imu.data.euler, 8);
BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &euler_frame);
// // 发送四元数数据 (ID: 0x69) - 前8字节 (q0, q1)
// BSP_CAN_StdDataFrame_t quat_frame = {
// .id = 0x69,
// .dlc = 8,
// .data = {0}
// };
// memcpy(quat_frame.data, &dm_imu.data.quat, 8);
// BSP_CAN_TransmitStdDataFrame(BSP_CAN_1, &quat_frame);
}
/* USER CODE END */ /* USER CODE END */
osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */ osDelayUntil(tick); /* 运行结束，等待下一次唤醒 */
} }

1
User/task/init.c
View File

@ -33,7 +33,6 @@ void Task_Init(void *argument) {
task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink); task_runtime.thread.blink = osThreadNew(Task_blink, NULL, &attr_blink);
task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu); task_runtime.thread.imu = osThreadNew(Task_imu, NULL, &attr_imu);
task_runtime.thread.ctrl_lz = osThreadNew(Task_ctrl_lz, NULL, &attr_ctrl_lz); task_runtime.thread.ctrl_lz = osThreadNew(Task_ctrl_lz, NULL, &attr_ctrl_lz);
task_runtime.thread.Task4 = osThreadNew(Task_Task4, NULL, &attr_Task4);
// 创建消息队列 // 创建消息队列
/* USER MESSAGE BEGIN */ /* USER MESSAGE BEGIN */

5
User/task/user_task.c
View File

@ -23,9 +23,4 @@ const osThreadAttr_t attr_ctrl_lz = {
.name = "ctrl_lz", .name = "ctrl_lz",
.priority = osPriorityNormal, .priority = osPriorityNormal,
.stack_size = 256 * 4, .stack_size = 256 * 4,
};
const osThreadAttr_t attr_Task4 = {
.name = "Task4",
.priority = osPriorityNormal,
.stack_size = 256 * 4,
}; };

8
User/task/user_task.h
View File

@ -16,14 +16,12 @@ extern "C" {
#define BLINK_FREQ (500.0) #define BLINK_FREQ (500.0)
#define IMU_FREQ (1000.0) #define IMU_FREQ (1000.0)
#define CTRL_LZ_FREQ (500.0) #define CTRL_LZ_FREQ (500.0)
#define TASK4_FREQ (500.0)
/* 任务初始化延时ms */ /* 任务初始化延时ms */
#define TASK_INIT_DELAY (100u) #define TASK_INIT_DELAY (100u)
#define BLINK_INIT_DELAY (0) #define BLINK_INIT_DELAY (0)
#define IMU_INIT_DELAY (0) #define IMU_INIT_DELAY (0)
#define CTRL_LZ_INIT_DELAY (0) #define CTRL_LZ_INIT_DELAY (0)
#define TASK4_INIT_DELAY (0)
/* Exported defines --------------------------------------------------------- */ /* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */ /* Exported macro ----------------------------------------------------------- */
@ -36,7 +34,6 @@ typedef struct {
osThreadId_t blink; osThreadId_t blink;
osThreadId_t imu; osThreadId_t imu;
osThreadId_t ctrl_lz; osThreadId_t ctrl_lz;
osThreadId_t Task4;
} thread; } thread;
/* USER MESSAGE BEGIN */ /* USER MESSAGE BEGIN */
@ -61,7 +58,6 @@ typedef struct {
UBaseType_t blink; UBaseType_t blink;
UBaseType_t imu; UBaseType_t imu;
UBaseType_t ctrl_lz; UBaseType_t ctrl_lz;
UBaseType_t Task4;
} stack_water_mark; } stack_water_mark;
/* 各任务运行频率 */ /* 各任务运行频率 */
@ -69,7 +65,6 @@ typedef struct {
float blink; float blink;
float imu; float imu;
float ctrl_lz; float ctrl_lz;
float Task4;
} freq; } freq;
/* 任务最近运行时间 */ /* 任务最近运行时间 */
@ -77,7 +72,6 @@ typedef struct {
float blink; float blink;
float imu; float imu;
float ctrl_lz; float ctrl_lz;
float Task4;
} last_up_time; } last_up_time;
} Task_Runtime_t; } Task_Runtime_t;
@ -90,14 +84,12 @@ extern const osThreadAttr_t attr_init;
extern const osThreadAttr_t attr_blink; extern const osThreadAttr_t attr_blink;
extern const osThreadAttr_t attr_imu; extern const osThreadAttr_t attr_imu;
extern const osThreadAttr_t attr_ctrl_lz; extern const osThreadAttr_t attr_ctrl_lz;
extern const osThreadAttr_t attr_Task4;
/* 任务函数声明 */ /* 任务函数声明 */
void Task_Init(void *argument); void Task_Init(void *argument);
void Task_blink(void *argument); void Task_blink(void *argument);
void Task_imu(void *argument); void Task_imu(void *argument);
void Task_ctrl_lz(void *argument); void Task_ctrl_lz(void *argument);
void Task_Task4(void *argument);
#ifdef __cplusplus #ifdef __cplusplus
} }