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-02-04 03:38:04 +08:00
parent 00451057cf
commit a2c06026f3
11 changed files with 519 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
CMakeLists.txt
View File

@ -82,6 +82,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/device/motor_rm.c
User/device/vision_bridge.c
User/device/vofa.c
User/device/mrobot.c
# User/module sources
User/module/balance_chassis.c
@ -99,6 +100,7 @@ target_sources(${CMAKE_PROJECT_NAME} PRIVATE
User/task/init.c
User/task/monitor.c
User/task/rc.c
User/task/cli.c
User/task/user_task.c
User/task/vofa.c
)

2
Core/Inc/FreeRTOSConfig.h
View File

@ -72,7 +72,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)0x20000)
#define configMAX_TASK_NAME_LEN ( 16 )
#define configGENERATE_RUN_TIME_STATS 1
#define configUSE_TRACE_FACILITY 1

3
User/component/freertos_cli.h
View File

@ -28,6 +28,9 @@
#ifndef COMMAND_INTERPRETER_H
#define COMMAND_INTERPRETER_H
#include <stddef.h>
#include "FreeRTOS.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

272
User/device/mrobot.c Normal file
View File

@ -0,0 +1,272 @@
/* Includes ----------------------------------------------------------------- */
#include "device/mrobot.h"
#include "component/freertos_cli.h"
#include "bsp/uart.h"
#include <string.h>
#include <stdio.h>
#include <FreeRTOS.h>
#include <task.h>
/* Private variables -------------------------------------------------------- */
static MRobot_Device_t devices[MROBOT_MAX_DEVICES];
static uint8_t device_count = 0;
static char current_path[64] = "/";
static char output_buffer[MROBOT_MAX_OUTPUT_LEN];
/* Private function prototypes ---------------------------------------------- */
static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t cmd_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t cmd_cd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t cmd_ls(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
static BaseType_t cmd_pwd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
/* CLI 命令定义 */
static const CLI_Command_Definition_t cmd_def_help = {
"help",
"help: 显示所有可用命令\r\n",
cmd_help,
0
};
static const CLI_Command_Definition_t cmd_def_htop = {
"htop",
"htop: 显示 FreeRTOS 任务状态信息\r\n",
cmd_htop,
0
};
static const CLI_Command_Definition_t cmd_def_cd = {
"cd",
"cd <path>: 切换目录\r\n",
cmd_cd,
1
};
static const CLI_Command_Definition_t cmd_def_ls = {
"ls",
"ls: 列出当前目录内容\r\n",
cmd_ls,
0
};
static const CLI_Command_Definition_t cmd_def_show = {
"show",
"show [device]: 显示设备信息\r\n",
cmd_show,
-1 /* 可变参数 */
};
static const CLI_Command_Definition_t cmd_def_pwd = {
"pwd",
"pwd: 显示当前目录\r\n",
cmd_pwd,
0
};
/* Private functions -------------------------------------------------------- */
/* help 命令实现 */
static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
(void)pcCommandString;
snprintf(pcWriteBuffer, xWriteBufferLen,
"MRobot CLI v1.0\r\n"
"可用命令:\r\n"
" help - 显示帮助信息\r\n"
" htop - 显示任务状态\r\n"
" cd - 切换目录\r\n"
" ls - 列出目录内容\r\n"
" pwd - 显示当前路径\r\n"
" show - 显示设备信息\r\n"
"\r\n");
return pdFALSE;
}
/* htop 命令实现 */
static BaseType_t cmd_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
(void)pcCommandString;
static char task_buffer[512];
vTaskList(task_buffer);
snprintf(pcWriteBuffer, xWriteBufferLen,
"Task Name\tState\tPri\tStack\tNum\r\n"
"-----------------------------------------------\r\n"
"%s\r\n", task_buffer);
return pdFALSE;
}
/* pwd 命令实现 */
static BaseType_t cmd_pwd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
(void)pcCommandString;
snprintf(pcWriteBuffer, xWriteBufferLen, "%s\r\n", current_path);
return pdFALSE;
}
/* cd 命令实现 */
static BaseType_t cmd_cd(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
const char *param;
BaseType_t param_len;
param = FreeRTOS_CLIGetParameter(pcCommandString, 1, &param_len);
if (param != NULL) {
char path[64];
strncpy(path, param, param_len);
path[param_len] = '\0';
if (strcmp(path, "/") == 0 || strcmp(path, "..") == 0) {
strcpy(current_path, "/");
} else if (strcmp(path, "dev") == 0 || strcmp(path, "/dev") == 0) {
strcpy(current_path, "/dev");
} else if (strcmp(path, "modules") == 0 || strcmp(path, "/modules") == 0) {
strcpy(current_path, "/modules");
} else {
snprintf(pcWriteBuffer, xWriteBufferLen, "错误: 目录不存在\r\n");
return pdFALSE;
}
snprintf(pcWriteBuffer, xWriteBufferLen, "切换到: %s\r\n", current_path);
} else {
snprintf(pcWriteBuffer, xWriteBufferLen, "错误: 缺少路径参数\r\n");
}
return pdFALSE;
}
/* ls 命令实现 */
static BaseType_t cmd_ls(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
(void)pcCommandString;
int offset = 0;
if (strcmp(current_path, "/") == 0) {
offset = snprintf(pcWriteBuffer, xWriteBufferLen, "dev/\r\nmodules/\r\n");
} else if (strcmp(current_path, "/dev") == 0) {
offset = snprintf(pcWriteBuffer, xWriteBufferLen, "设备列表:\r\n");
for (uint8_t i = 0; i < device_count && offset < (int)xWriteBufferLen; i++) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" %s\r\n", devices[i].name);
}
if (device_count == 0) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" (无设备)\r\n");
}
} else if (strcmp(current_path, "/modules") == 0) {
offset = snprintf(pcWriteBuffer, xWriteBufferLen, "(模块功能暂未实现)\r\n");
}
return pdFALSE;
}
/* show 命令实现 */
static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
const char *param;
BaseType_t param_len;
param = FreeRTOS_CLIGetParameter(pcCommandString, 1, &param_len);
if (strcmp(current_path, "/dev") != 0) {
snprintf(pcWriteBuffer, xWriteBufferLen, "错误: show 命令仅在 /dev 目录下可用\r\n");
return pdFALSE;
}
if (param == NULL) {
/* 显示所有设备 */
int offset = snprintf(pcWriteBuffer, xWriteBufferLen, "所有设备信息:\r\n");
for (uint8_t i = 0; i < device_count && offset < (int)xWriteBufferLen; i++) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
"\r\n=== %s ===\r\n", devices[i].name);
if (devices[i].print_callback != NULL) {
devices[i].print_callback(devices[i].data,
pcWriteBuffer + offset,
xWriteBufferLen - offset);
offset = strlen(pcWriteBuffer);
}
}
} else {
/* 显示特定设备 */
char device_name[32];
strncpy(device_name, param, param_len);
device_name[param_len] = '\0';
bool found = false;
for (uint8_t i = 0; i < device_count; i++) {
if (strcmp(devices[i].name, device_name) == 0) {
found = true;
int offset = snprintf(pcWriteBuffer, xWriteBufferLen,
"=== %s ===\r\n", devices[i].name);
if (devices[i].print_callback != NULL) {
devices[i].print_callback(devices[i].data,
pcWriteBuffer + offset,
xWriteBufferLen - offset);
} else {
snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
"无打印函数\r\n");
}
break;
}
}
if (!found) {
snprintf(pcWriteBuffer, xWriteBufferLen, "错误: 设备 '%s' 未找到\r\n", device_name);
}
}
return pdFALSE;
}
/* Exported functions ------------------------------------------------------- */
void MRobot_Init(void) {
/* 初始化设备数组 */
memset(devices, 0, sizeof(devices));
device_count = 0;
/* 注册 CLI 命令 */
FreeRTOS_CLIRegisterCommand(&cmd_def_help);
FreeRTOS_CLIRegisterCommand(&cmd_def_htop);
FreeRTOS_CLIRegisterCommand(&cmd_def_cd);
FreeRTOS_CLIRegisterCommand(&cmd_def_ls);
FreeRTOS_CLIRegisterCommand(&cmd_def_show);
FreeRTOS_CLIRegisterCommand(&cmd_def_pwd);
}
int8_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type,
void *data, MRobot_PrintCallback_t print_callback) {
if (device_count >= MROBOT_MAX_DEVICES) {
return -1;
}
if (name == NULL || data == NULL) {
return -1;
}
strncpy(devices[device_count].name, name, sizeof(devices[device_count].name) - 1);
devices[device_count].type = type;
devices[device_count].data = data;
devices[device_count].print_callback = print_callback;
device_count++;
return 0;
}
int8_t MRobot_ProcessCommand(const char *cmd_string, char *output_buffer, uint16_t buffer_size) {
if (cmd_string == NULL || output_buffer == NULL || buffer_size == 0) {
return -1;
}
BaseType_t result;
int offset = 0;
do {
result = FreeRTOS_CLIProcessCommand(cmd_string,
output_buffer + offset,
buffer_size - offset);
offset = strlen(output_buffer);
} while (result != pdFALSE && offset < (int)buffer_size);
return 0;
}
const char *MRobot_GetCurrentPath(void) {
return current_path;
}

95
User/device/mrobot.h Normal file
View File

@ -0,0 +1,95 @@
/*基于freertos_cli实现的虚拟命令行
自身作为一个设备存在，通过串口和上位机进行交互，获得类似命令行的体验，
例如输入help可以获得帮助
输出htop 可以获得freertos任务状态等信息
通过cd可以切换目录
目录结构是
/ (root)
|-- /dev
|-- bmi088
|-- chassis_motor1
|-- /modules
|-- balance_chassis
|-- gimbal
|-- shoot
在dev里可以用show命令查看设备信息
modules暂无实现
至于dev里的设备有哪些，需要通过一个函数MRobot_RegisterDevices(设备名（字符串），结构体指针)来注册设备，我可以在任何一个任务里调用这个函数。
然后我在我的一个单独的线程里打印和发送他们。
例如我在atti_esit任务里注册了bmi088和他的结构体，我在cli线程里需要能够打印bmi088的数据。
然后我在show的时候就可以看到imu的数据了，
*/
#pragma once
#ifdef __cplusplus
extern "C" {
#endif
/* Includes ----------------------------------------------------------------- */
#include <stdint.h>
#include <stdbool.h>
/* Exported constants ------------------------------------------------------- */
#define MROBOT_MAX_DEVICES 32
#define MROBOT_MAX_CMD_LEN 128
#define MROBOT_MAX_OUTPUT_LEN 512
#define MROBOT_RX_BUFFER_SIZE 256
/* Exported types ----------------------------------------------------------- */
typedef enum {
MROBOT_DEVICE_TYPE_IMU,
MROBOT_DEVICE_TYPE_MOTOR,
MROBOT_DEVICE_TYPE_SENSOR,
MROBOT_DEVICE_TYPE_CUSTOM,
} MRobot_DeviceType_t;
/* 设备打印回调函数类型 */
typedef void (*MRobot_PrintCallback_t)(void *device_data, char *buffer, uint16_t buffer_size);
/* 注册的设备结构 */
typedef struct {
char name[32];
MRobot_DeviceType_t type;
void *data; /* 指向实际设备数据的指针 */
MRobot_PrintCallback_t print_callback; /* 打印设备信息的回调函数 */
} MRobot_Device_t;
/* Exported functions ------------------------------------------------------- */
/**
* @brief 初始化 MRobot 命令行系统
*/
void MRobot_Init(void);
/**
* @brief 注册设备到 MRobot 系统
* @param name 设备名称
* @param type 设备类型
* @param data 指向设备数据结构的指针
* @param print_callback 打印设备信息的回调函数
* @retval 0 成功, -1 失败
*/
int8_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type,
void *data, MRobot_PrintCallback_t print_callback);
/**
* @brief 处理命令字符串
* @param cmd_string 命令字符串
* @param output_buffer 输出缓冲区
* @param buffer_size 缓冲区大小
* @retval 0 成功, -1 失败
*/
int8_t MRobot_ProcessCommand(const char *cmd_string, char *output_buffer, uint16_t buffer_size);
/**
* @brief 获取当前路径
* @return 当前路径字符串
*/
const char *MRobot_GetCurrentPath(void);
#ifdef __cplusplus
}
#endif

21
User/task/atti_esit.c
View File

@ -13,15 +13,13 @@
#include "component/QuaternionEKF.h"
#include "component/pid.h"
#include "device/bmi088.h"
#include "device/mrobot.h"
#include "module/balance_chassis.h"
#include "task/user_task.h"
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* 姿态解算算法选择: 定义USE_QEKF使用EKF算法，注释掉则使用AHRS算法 */
#define USE_QEKF
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
@ -52,6 +50,20 @@ static const KPID_Params_t imu_temp_ctrl_pid_param = {
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* BMI088 设备信息打印回调 */
static void bmi088_print_callback(void *device_data, char *buffer, uint16_t buffer_size) {
BMI088_t *imu = (BMI088_t *)device_data;
snprintf(buffer, buffer_size,
"加速度计: X=%.3f Y=%.3f Z=%.3f m/s²\r\n"
"陀螺仪: X=%.3f Y=%.3f Z=%.3f rad/s\r\n"
"温度: %.2f °C\r\n"
"欧拉角: Roll=%.2f Pitch=%.2f Yaw=%.2f °\r\n",
imu->accl.x, imu->accl.y, imu->accl.z,
imu->gyro.x, imu->gyro.y, imu->gyro.z,
imu->temp,
eulr_to_send.rol * 57.2958f, eulr_to_send.pit * 57.2958f, eulr_to_send.yaw * 57.2958f);
}
/* Exported functions ------------------------------------------------------- */
void Task_atti_esit(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
@ -69,6 +81,9 @@ void Task_atti_esit(void *argument) {
PID_Init(&imu_temp_ctrl_pid, KPID_MODE_NO_D,
1.0f / BMI088_GetUpdateFreq(&bmi088), &imu_temp_ctrl_pid_param);
BSP_PWM_Start(BSP_PWM_IMU_HEAT);
/* 注册 BMI088 设备到 MRobot */
MRobot_RegisterDevice("bmi088", MROBOT_DEVICE_TYPE_IMU, &bmi088, bmi088_print_callback);
/* USER CODE INIT END */
while (1) {

111
User/task/cli.c Normal file
View File

@ -0,0 +1,111 @@
/*
cli Task
*/
/* Includes ----------------------------------------------------------------- */
#include "task/user_task.h"
/* USER INCLUDE BEGIN */
#include "device/mrobot.h"
#include "bsp/uart.h"
#include <string.h>
#include <stdbool.h>
/* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */
/* Private define ----------------------------------------------------------- */
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */
static uint8_t uart_rx_char;
static uint8_t cmd_buffer[128];
static volatile uint8_t cmd_index = 0;
static volatile bool cmd_ready = false;
static char output_buffer[512];
/* USER STRUCT END */
/* Private function --------------------------------------------------------- */
/* UART 接收中断回调 */
static void cli_uart_rx_callback(void) {
uint8_t ch = uart_rx_char;
if (ch == '\r' || ch == '\n') {
if (cmd_index > 0) {
cmd_buffer[cmd_index] = '\0';
cmd_ready = true;
/* 发送换行 */
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)"\r\n", 2, false);
}
} else if (ch == 127 || ch == 8) { /* 退格键 */
if (cmd_index > 0) {
cmd_index--;
/* 回显退格 */
const char *backspace = "\b \b";
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)backspace, 3, false);
}
} else if (ch >= 32 && ch < 127 && cmd_index < sizeof(cmd_buffer) - 1) {
/* 只接受可打印字符 */
cmd_buffer[cmd_index++] = ch;
/* 回显字符 */
BSP_UART_Transmit(BSP_UART_VOFA, &ch, 1, false);
}
/* 重新启动接收 */
BSP_UART_Receive(BSP_UART_VOFA, &uart_rx_char, 1, false);
}
/* Exported functions ------------------------------------------------------- */
void Task_cli(void *argument) {
(void)argument; /* 未使用argument，消除警告 */
osDelay(CLI_INIT_DELAY); /* 延时一段时间再开启任务 */
/* USER CODE INIT BEGIN */
/* 初始化 MRobot CLI 系统 */
MRobot_Init();
/* 注册 UART 接收回调 */
BSP_UART_RegisterCallback(BSP_UART_VOFA, BSP_UART_RX_CPLT_CB, cli_uart_rx_callback);
/* 启动 UART 接收 */
BSP_UART_Receive(BSP_UART_VOFA, &uart_rx_char, 1, false);
/* 发送欢迎消息 */
const char *welcome = "\r\n\r\n"
"================================\r\n"
" MRobot CLI v1.0\r\n"
" 输入 'help' 获取帮助\r\n"
"================================\r\n"
"root@mrobot:~$ ";
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)welcome, strlen(welcome), false);
/* USER CODE INIT END */
while (1) {
/* USER CODE BEGIN */
/* 检查是否有命令需要处理 */
if (cmd_ready) {
/* 处理命令 */
if (MRobot_ProcessCommand((char *)cmd_buffer, output_buffer, sizeof(output_buffer)) == 0) {
if (strlen(output_buffer) > 0) {
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)output_buffer, strlen(output_buffer), false);
osDelay(10); /* 等待发送完成 */
}
}
/* 发送提示符 */
char prompt[128];
snprintf(prompt, sizeof(prompt), "root@mrobot:%s$ ", MRobot_GetCurrentPath());
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)prompt, strlen(prompt), false);
osDelay(5); /* 等待发送完成 */
/* 重置状态 */
cmd_index = 0;
cmd_ready = false;
}
osDelay(10);
/* USER CODE END */
}
}

6
User/task/config.yaml
View File

@ -60,3 +60,9 @@
function: Task_vofa
name: vofa
stack: 256
- delay: 0
description: ''
freq_control: false
function: Task_cli
name: cli
stack: 1024

1
User/task/init.c
View File

@ -41,6 +41,7 @@ void Task_Init(void *argument) {
task_runtime.thread.ctrl_shoot = osThreadNew(Task_ctrl_shoot, NULL, &attr_ctrl_shoot);
task_runtime.thread.ai = osThreadNew(Task_ai, NULL, &attr_ai);
// task_runtime.thread.vofa = osThreadNew(Task_vofa, NULL, &attr_vofa);
task_runtime.thread.cli = osThreadNew(Task_cli, NULL, &attr_cli);
// 创建消息队列
/* USER MESSAGE BEGIN */

5
User/task/user_task.c
View File

@ -54,3 +54,8 @@ const osThreadAttr_t attr_vofa = {
.priority = osPriorityNormal,
.stack_size = 256 * 4,
};
const osThreadAttr_t attr_cli = {
.name = "cli",
.priority = osPriorityNormal,
.stack_size = 256 * 12,
};

5
User/task/user_task.h
View File

@ -33,6 +33,7 @@ extern "C" {
#define CTRL_SHOOT_INIT_DELAY (0)
#define AI_INIT_DELAY (0)
#define VOFA_INIT_DELAY (0)
#define CLI_INIT_DELAY (0)
/* Exported defines --------------------------------------------------------- */
/* Exported macro ----------------------------------------------------------- */
@ -51,6 +52,7 @@ typedef struct {
osThreadId_t ctrl_shoot;
osThreadId_t ai;
osThreadId_t vofa;
osThreadId_t cli;
} thread;
/* USER MESSAGE BEGIN */
@ -106,6 +108,7 @@ typedef struct {
UBaseType_t ctrl_shoot;
UBaseType_t ai;
UBaseType_t vofa;
UBaseType_t cli;
} stack_water_mark;
/* 各任务运行频率 */
@ -148,6 +151,7 @@ extern const osThreadAttr_t attr_blink;
extern const osThreadAttr_t attr_ctrl_shoot;
extern const osThreadAttr_t attr_ai;
extern const osThreadAttr_t attr_vofa;
extern const osThreadAttr_t attr_cli;
/* 任务函数声明 */
void Task_Init(void *argument);
@ -160,6 +164,7 @@ void Task_blink(void *argument);
void Task_ctrl_shoot(void *argument);
void Task_ai(void *argument);
void Task_vofa(void *argument);
void Task_cli(void *argument);
#ifdef __cplusplus
}