可以读imu了
This commit is contained in:
parent
a2c06026f3
commit
ff40b71c75
@ -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;
|
||||||
|
|||||||
@ -1,5 +1,6 @@
|
|||||||
#pragma once
|
#pragma once
|
||||||
|
|
||||||
|
#include <sys/_intsup.h>
|
||||||
#ifdef __cplusplus
|
#ifdef __cplusplus
|
||||||
extern "C" {
|
extern "C" {
|
||||||
#endif
|
#endif
|
||||||
|
|||||||
@ -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;
|
||||||
|
}
|
||||||
|
|
||||||
|
/* 检查是否有命令需要处理 */
|
||||||
|
if (cmd_ready) {
|
||||||
|
/* 检查是否是 htop 命令 */
|
||||||
|
if (strcmp((char *)cmd_buffer, "htop") == 0) {
|
||||||
|
htop_mode = true;
|
||||||
|
htop_exit = false;
|
||||||
|
} else {
|
||||||
|
/* 处理其他命令 */
|
||||||
BaseType_t result;
|
BaseType_t result;
|
||||||
int offset = 0;
|
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);
|
||||||
|
while (!tx_complete) { osDelay(1); } /* 等待发送完成 */
|
||||||
|
}
|
||||||
|
|
||||||
const char *MRobot_GetCurrentPath(void) {
|
/* 发送提示符 */
|
||||||
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);
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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
|
||||||
}
|
}
|
||||||
|
|||||||
@ -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) {
|
||||||
|
|||||||
@ -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 */
|
||||||
}
|
}
|
||||||
|
|
||||||
|
|||||||
@ -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,
|
||||||
};
|
};
|
||||||
Loading…
Reference in New Issue
Block a user