可以读imu了

This commit is contained in:
Robofish 2026-02-04 04:10:37 +08:00
parent a2c06026f3
commit ff40b71c75
7 changed files with 261 additions and 126 deletions

View File

@ -1,5 +1,6 @@
#pragma once #pragma once
#include <sys/_intsup.h>
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif
@ -30,6 +31,7 @@ typedef struct {
} gyro_offset; /* 陀螺仪偏置 */ } gyro_offset; /* 陀螺仪偏置 */
} BMI088_Cali_t; /* BMI088校准数据 */ } BMI088_Cali_t; /* BMI088校准数据 */
typedef struct { typedef struct {
DEVICE_Header_t header; DEVICE_Header_t header;
AHRS_Accl_t accl; AHRS_Accl_t accl;

View File

@ -1,5 +1,6 @@
#pragma once #pragma once
#include <sys/_intsup.h>
#ifdef __cplusplus #ifdef __cplusplus
extern "C" { extern "C" {
#endif #endif

View File

@ -13,6 +13,15 @@ static uint8_t device_count = 0;
static char current_path[64] = "/"; static char current_path[64] = "/";
static char output_buffer[MROBOT_MAX_OUTPUT_LEN]; static char output_buffer[MROBOT_MAX_OUTPUT_LEN];
/* UART 相关变量 */
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 volatile bool htop_mode = false;
static volatile bool htop_exit = false;
static volatile bool tx_complete = true;
/* Private function prototypes ---------------------------------------------- */ /* Private function prototypes ---------------------------------------------- */
static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString); 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_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString);
@ -31,7 +40,7 @@ static const CLI_Command_Definition_t cmd_def_help = {
static const CLI_Command_Definition_t cmd_def_htop = { static const CLI_Command_Definition_t cmd_def_htop = {
"htop", "htop",
"htop: 显示 FreeRTOS 任务状态信息\r\n", "htop: 动态显示 FreeRTOS 任务状态 (按 'q' 退出)\r\n",
cmd_htop, cmd_htop,
0 0
}; };
@ -73,7 +82,7 @@ static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const ch
"MRobot CLI v1.0\r\n" "MRobot CLI v1.0\r\n"
"可用命令:\r\n" "可用命令:\r\n"
" help - 显示帮助信息\r\n" " help - 显示帮助信息\r\n"
" htop - 显示任务状态\r\n" " htop - 动态显示任务状态 (按 'q' 退出)\r\n"
" cd - 切换目录\r\n" " cd - 切换目录\r\n"
" ls - 列出目录内容\r\n" " ls - 列出目录内容\r\n"
" pwd - 显示当前路径\r\n" " pwd - 显示当前路径\r\n"
@ -82,17 +91,12 @@ static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const ch
return pdFALSE; return pdFALSE;
} }
/* htop 命令实现 */ /* htop 命令实现 - 由 cli.c 处理动态显示 */
static BaseType_t cmd_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) { static BaseType_t cmd_htop(char *pcWriteBuffer, size_t xWriteBufferLen, const char *pcCommandString) {
(void)pcCommandString; (void)pcCommandString;
static char task_buffer[512]; (void)pcWriteBuffer;
(void)xWriteBufferLen;
vTaskList(task_buffer); /* htop 命令在 cli.c 中被特殊处理,这里返回空 */
snprintf(pcWriteBuffer, xWriteBufferLen,
"Task Name\tState\tPri\tStack\tNum\r\n"
"-----------------------------------------------\r\n"
"%s\r\n", task_buffer);
return pdFALSE; return pdFALSE;
} }
@ -214,6 +218,50 @@ static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const ch
return pdFALSE; return pdFALSE;
} }
/* UART 发送完成回调 */
static void mrobot_uart_tx_callback(void) {
tx_complete = true;
}
/* UART 接收中断回调 */
static void mrobot_uart_rx_callback(void) {
uint8_t ch = uart_rx_char;
/* 如果在 htop 模式,检查退出键 */
if (htop_mode) {
if (ch == 'q' || ch == 'Q' || ch == 27) { /* q 或 ESC */
htop_exit = true;
}
/* 重新启动接收 */
BSP_UART_Receive(BSP_UART_VOFA, &uart_rx_char, 1, false);
return;
}
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 ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
void MRobot_Init(void) { void MRobot_Init(void) {
@ -228,6 +276,24 @@ void MRobot_Init(void) {
FreeRTOS_CLIRegisterCommand(&cmd_def_ls); FreeRTOS_CLIRegisterCommand(&cmd_def_ls);
FreeRTOS_CLIRegisterCommand(&cmd_def_show); FreeRTOS_CLIRegisterCommand(&cmd_def_show);
FreeRTOS_CLIRegisterCommand(&cmd_def_pwd); FreeRTOS_CLIRegisterCommand(&cmd_def_pwd);
/* 注册 UART 回调 */
BSP_UART_RegisterCallback(BSP_UART_VOFA, BSP_UART_RX_CPLT_CB, mrobot_uart_rx_callback);
BSP_UART_RegisterCallback(BSP_UART_VOFA, BSP_UART_TX_CPLT_CB, mrobot_uart_tx_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:~$ ";
tx_complete = false;
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)welcome, strlen(welcome), true);
while (!tx_complete) { osDelay(1); } /* 等待发送完成 */
} }
int8_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type, int8_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type,
@ -249,24 +315,129 @@ int8_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type,
return 0; return 0;
} }
int8_t MRobot_ProcessCommand(const char *cmd_string, char *output_buffer, uint16_t buffer_size) { void MRobot_Run(void) {
if (cmd_string == NULL || output_buffer == NULL || buffer_size == 0) { /* 动态 htop 模式 */
return -1; if (htop_mode) {
/* 清屏并重置光标 */
const char *clear = "\033[2J\033[H";
tx_complete = false;
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)clear, strlen(clear), true);
while (!tx_complete) { osDelay(1); } /* 等待发送完成 */
/* 显示 htop 头部 */
const char *header =
"MRobot Task Monitor (按 'q' 退出)\r\n"
"================================================================================\r\n";
tx_complete = false;
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)header, strlen(header), true);
while (!tx_complete) { osDelay(1); } /* 等待发送完成 */
/* 获取任务列表 */
char task_buffer[1024];
vTaskList(task_buffer);
/* 格式化输出 */
char display_buffer[1536];
int offset = snprintf(display_buffer, sizeof(display_buffer),
"%-16s %-8s %-4s %-8s %-4s\r\n"
"--------------------------------------------------------------------------------\r\n",
"Task Name", "State", "Prio", "Stack", "Num");
/* 解析并美化任务列表 */
char *line = strtok(task_buffer, "\r\n");
while (line != NULL && offset < (int)sizeof(display_buffer) - 100) {
char name[17] = {0};
char state[9] = {0};
int prio = 0;
int stack = 0;
int num = 0;
if (sscanf(line, "%16s %c %d %d %d", name, &state[0], &prio, &stack, &num) == 5) {
/* 状态字符转换 */
const char *state_str = "?";
switch(state[0]) {
case 'R': state_str = "Running"; break;
case 'B': state_str = "Blocked"; break;
case 'S': state_str = "Suspend"; break;
case 'D': state_str = "Deleted"; break;
default: state_str = "Unknown"; break;
}
offset += snprintf(display_buffer + offset, sizeof(display_buffer) - offset,
"%-16s %-8s %-4d %-8d %-4d\r\n",
name, state_str, prio, stack, num);
}
line = strtok(NULL, "\r\n");
}
/* 添加运行时统计 */
offset += snprintf(display_buffer + offset, sizeof(display_buffer) - offset,
"--------------------------------------------------------------------------------\r\n"
"System Tick: %lu\r\n",
(unsigned long)xTaskGetTickCount());
tx_complete = false;
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)display_buffer, strlen(display_buffer), true);
while (!tx_complete) { osDelay(1); } /* 等待发送完成 */
/* 检查退出标志 */
if (htop_exit) {
htop_mode = false;
htop_exit = false;
/* 清屏 */
tx_complete = false;
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)clear, strlen(clear), true);
while (!tx_complete) { osDelay(1); } /* 等待发送完成 */
/* 显示提示符 */
char prompt[128];
snprintf(prompt, sizeof(prompt), "root@mrobot:%s$ ", current_path);
tx_complete = false;
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)prompt, strlen(prompt), true);
while (!tx_complete) { osDelay(1); } /* 等待发送完成 */
}
osDelay(200); /* 每 200ms 刷新一次5fps */
return;
} }
BaseType_t result; /* 检查是否有命令需要处理 */
int offset = 0; if (cmd_ready) {
/* 检查是否是 htop 命令 */
if (strcmp((char *)cmd_buffer, "htop") == 0) {
htop_mode = true;
htop_exit = false;
} else {
/* 处理其他命令 */
BaseType_t result;
int offset = 0;
do { do {
result = FreeRTOS_CLIProcessCommand(cmd_string, result = FreeRTOS_CLIProcessCommand((char *)cmd_buffer,
output_buffer + offset, output_buffer + offset,
buffer_size - offset); sizeof(output_buffer) - offset);
offset = strlen(output_buffer); offset = strlen(output_buffer);
} while (result != pdFALSE && offset < (int)buffer_size); } while (result != pdFALSE && offset < (int)sizeof(output_buffer));
return 0; if (strlen(output_buffer) > 0) {
} tx_complete = false;
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)output_buffer, strlen(output_buffer), true);
const char *MRobot_GetCurrentPath(void) { while (!tx_complete) { osDelay(1); } /* 等待发送完成 */
return current_path; }
/* 发送提示符 */
char prompt[128];
snprintf(prompt, sizeof(prompt), "root@mrobot:%s$ ", current_path);
tx_complete = false;
BSP_UART_Transmit(BSP_UART_VOFA, (uint8_t *)prompt, strlen(prompt), true);
while (!tx_complete) { osDelay(1); } /* 等待发送完成 */
}
/* 重置状态 */
cmd_index = 0;
cmd_ready = false;
}
osDelay(10);
} }

View File

@ -60,7 +60,7 @@ typedef struct {
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
/** /**
* @brief MRobot * @brief MRobot UART
*/ */
void MRobot_Init(void); void MRobot_Init(void);
@ -76,19 +76,9 @@ int8_t MRobot_RegisterDevice(const char *name, MRobot_DeviceType_t type,
void *data, MRobot_PrintCallback_t print_callback); void *data, MRobot_PrintCallback_t print_callback);
/** /**
* @brief * @brief MRobot CLI
* @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); void MRobot_Run(void);
/**
* @brief
* @return
*/
const char *MRobot_GetCurrentPath(void);
#ifdef __cplusplus #ifdef __cplusplus
} }

View File

@ -16,6 +16,7 @@
#include "device/mrobot.h" #include "device/mrobot.h"
#include "module/balance_chassis.h" #include "module/balance_chassis.h"
#include "task/user_task.h" #include "task/user_task.h"
#include <stdlib.h>
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -33,6 +34,17 @@ AHRS_Eulr_t eulr_to_send;
Chassis_IMU_t imu_for_chassis; Chassis_IMU_t imu_for_chassis;
KPID_t imu_temp_ctrl_pid; KPID_t imu_temp_ctrl_pid;
/* 用于 MRobot 显示的 IMU 数据结构 */
typedef struct {
BMI088_t *bmi088;
AHRS_Eulr_t *euler;
} IMU_Display_Data_t;
static IMU_Display_Data_t imu_display_data = {
.bmi088 = &bmi088,
.euler = &eulr_to_send,
};
/*默认校准参数*/ /*默认校准参数*/
BMI088_Cali_t cali_bmi088 = { BMI088_Cali_t cali_bmi088 = {
.gyro_offset = {-0.00147764047f, -0.00273479894f, 0.00154074503f}, .gyro_offset = {-0.00147764047f, -0.00273479894f, 0.00154074503f},
@ -52,16 +64,46 @@ static const KPID_Params_t imu_temp_ctrl_pid_param = {
/* Private function --------------------------------------------------------- */ /* Private function --------------------------------------------------------- */
/* BMI088 设备信息打印回调 */ /* BMI088 设备信息打印回调 */
static void bmi088_print_callback(void *device_data, char *buffer, uint16_t buffer_size) { static void bmi088_print_callback(void *device_data, char *buffer, uint16_t buffer_size) {
BMI088_t *imu = (BMI088_t *)device_data; IMU_Display_Data_t *data = (IMU_Display_Data_t *)device_data;
/* 将浮点数转换为整数和小数部分,避免 printf 浮点数支持问题 */
int accl_x_int = (int)(data->bmi088->accl.x);
int accl_x_frac = (int)((data->bmi088->accl.x - accl_x_int) * 1000);
int accl_y_int = (int)(data->bmi088->accl.y);
int accl_y_frac = (int)((data->bmi088->accl.y - accl_y_int) * 1000);
int accl_z_int = (int)(data->bmi088->accl.z);
int accl_z_frac = (int)((data->bmi088->accl.z - accl_z_int) * 1000);
int gyro_x_int = (int)(data->bmi088->gyro.x);
int gyro_x_frac = (int)((data->bmi088->gyro.x - gyro_x_int) * 1000);
int gyro_y_int = (int)(data->bmi088->gyro.y);
int gyro_y_frac = (int)((data->bmi088->gyro.y - gyro_y_int) * 1000);
int gyro_z_int = (int)(data->bmi088->gyro.z);
int gyro_z_frac = (int)((data->bmi088->gyro.z - gyro_z_int) * 1000);
int temp_int = (int)(data->bmi088->temp);
int temp_frac = (int)((data->bmi088->temp - temp_int) * 100);
float roll_deg = data->euler->rol * 57.2958f;
float pitch_deg = data->euler->pit * 57.2958f;
float yaw_deg = data->euler->yaw * 57.2958f;
int roll_int = (int)roll_deg;
int roll_frac = (int)((roll_deg - roll_int) * 100);
int pitch_int = (int)pitch_deg;
int pitch_frac = (int)((pitch_deg - pitch_int) * 100);
int yaw_int = (int)yaw_deg;
int yaw_frac = (int)((yaw_deg - yaw_int) * 100);
snprintf(buffer, buffer_size, snprintf(buffer, buffer_size,
"加速度计: X=%.3f Y=%.3f Z=%.3f m/s²\r\n" "加速度计: X=%d.%03d Y=%d.%03d Z=%d.%03d m/s²\r\n"
"陀螺仪: X=%.3f Y=%.3f Z=%.3f rad/s\r\n" "陀螺仪: X=%d.%03d Y=%d.%03d Z=%d.%03d rad/s\r\n"
"温度: %.2f °C\r\n" "温度: %d.%02d °C\r\n"
"欧拉角: Roll=%.2f Pitch=%.2f Yaw=%.2f °\r\n", "欧拉角: Roll=%d.%02d Pitch=%d.%02d Yaw=%d.%02d °\r\n",
imu->accl.x, imu->accl.y, imu->accl.z, accl_x_int, abs(accl_x_frac), accl_y_int, abs(accl_y_frac), accl_z_int, abs(accl_z_frac),
imu->gyro.x, imu->gyro.y, imu->gyro.z, gyro_x_int, abs(gyro_x_frac), gyro_y_int, abs(gyro_y_frac), gyro_z_int, abs(gyro_z_frac),
imu->temp, temp_int, abs(temp_frac),
eulr_to_send.rol * 57.2958f, eulr_to_send.pit * 57.2958f, eulr_to_send.yaw * 57.2958f); roll_int, abs(roll_frac), pitch_int, abs(pitch_frac), yaw_int, abs(yaw_frac));
} }
/* Exported functions ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
@ -83,7 +125,7 @@ void Task_atti_esit(void *argument) {
BSP_PWM_Start(BSP_PWM_IMU_HEAT); BSP_PWM_Start(BSP_PWM_IMU_HEAT);
/* 注册 BMI088 设备到 MRobot */ /* 注册 BMI088 设备到 MRobot */
MRobot_RegisterDevice("bmi088", MROBOT_DEVICE_TYPE_IMU, &bmi088, bmi088_print_callback); MRobot_RegisterDevice("bmi088", MROBOT_DEVICE_TYPE_IMU, &imu_display_data, bmi088_print_callback);
/* USER CODE INIT END */ /* USER CODE INIT END */
while (1) { while (1) {

View File

@ -7,9 +7,6 @@
#include "task/user_task.h" #include "task/user_task.h"
/* USER INCLUDE BEGIN */ /* USER INCLUDE BEGIN */
#include "device/mrobot.h" #include "device/mrobot.h"
#include "bsp/uart.h"
#include <string.h>
#include <stdbool.h>
/* USER INCLUDE END */ /* USER INCLUDE END */
/* Private typedef ---------------------------------------------------------- */ /* Private typedef ---------------------------------------------------------- */
@ -17,43 +14,10 @@
/* Private macro ------------------------------------------------------------ */ /* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */ /* Private variables -------------------------------------------------------- */
/* USER STRUCT BEGIN */ /* 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 */ /* USER STRUCT END */
/* Private function --------------------------------------------------------- */ /* 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 ------------------------------------------------------- */ /* Exported functions ------------------------------------------------------- */
void Task_cli(void *argument) { void Task_cli(void *argument) {
(void)argument; /* 未使用argument消除警告 */ (void)argument; /* 未使用argument消除警告 */
@ -64,47 +28,12 @@ void Task_cli(void *argument) {
/* USER CODE INIT BEGIN */ /* USER CODE INIT BEGIN */
/* 初始化 MRobot CLI 系统 */ /* 初始化 MRobot CLI 系统 */
MRobot_Init(); 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 */ /* USER CODE INIT END */
while (1) { while (1) {
/* USER CODE BEGIN */ /* USER CODE BEGIN */
/* 检查是否有命令需要处理 */ /* 运行 MRobot 主循环 */
if (cmd_ready) { MRobot_Run();
/* 处理命令 */
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 */ /* USER CODE END */
} }

View File

@ -57,5 +57,5 @@ const osThreadAttr_t attr_vofa = {
const osThreadAttr_t attr_cli = { const osThreadAttr_t attr_cli = {
.name = "cli", .name = "cli",
.priority = osPriorityNormal, .priority = osPriorityNormal,
.stack_size = 256 * 12, .stack_size = 256 * 16,
}; };