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 13:45:07 +08:00
parent 20afc1a656
commit 268a132283
1 changed files with 67 additions and 53 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

118
User/device/mrobot.c
View File

@ -229,29 +229,45 @@ static int print_imu_device(const void *device_data, char *buffer, size_t buffer
char gx[16], gy[16], gz[16]; char gx[16], gy[16], gz[16];
char temp[16]; char temp[16];
char roll[16], pitch[16], yaw[16]; char roll[16], pitch[16], yaw[16];
char q0[16], q1[16], q2[16], q3[16];
/* 格式化加速度 */
format_float(ax, sizeof(ax), imu->accl.x, 3); format_float(ax, sizeof(ax), imu->accl.x, 3);
format_float(ay, sizeof(ay), imu->accl.y, 3); format_float(ay, sizeof(ay), imu->accl.y, 3);
format_float(az, sizeof(az), imu->accl.z, 3); format_float(az, sizeof(az), imu->accl.z, 3);
/* 格式化陀螺仪 */
format_float(gx, sizeof(gx), imu->gyro.x, 3); format_float(gx, sizeof(gx), imu->gyro.x, 3);
format_float(gy, sizeof(gy), imu->gyro.y, 3); format_float(gy, sizeof(gy), imu->gyro.y, 3);
format_float(gz, sizeof(gz), imu->gyro.z, 3); format_float(gz, sizeof(gz), imu->gyro.z, 3);
/* 格式化温度 */
format_float(temp, sizeof(temp), imu->temp, 2); format_float(temp, sizeof(temp), imu->temp, 2);
/* 格式化欧拉角（转换为角度） */
format_float(roll, sizeof(roll), imu->euler.rol * RAD_TO_DEG, 2); format_float(roll, sizeof(roll), imu->euler.rol * RAD_TO_DEG, 2);
format_float(pitch, sizeof(pitch), imu->euler.pit * RAD_TO_DEG, 2); format_float(pitch, sizeof(pitch), imu->euler.pit * RAD_TO_DEG, 2);
format_float(yaw, sizeof(yaw), imu->euler.yaw * RAD_TO_DEG, 2); format_float(yaw, sizeof(yaw), imu->euler.yaw * RAD_TO_DEG, 2);
/* 格式化四元数 */
format_float(q0, sizeof(q0), imu->quat.q0, 4);
format_float(q1, sizeof(q1), imu->quat.q1, 4);
format_float(q2, sizeof(q2), imu->quat.q2, 4);
format_float(q3, sizeof(q3), imu->quat.q3, 4);
return snprintf(buffer, buffer_size, return snprintf(buffer, buffer_size,
"状态: %s\r\n" " Status : %s\r\n"
"加速度计: X=%s Y=%s Z=%s m/s²\r\n" " Accel : X=%-10s Y=%-10s Z=%-10s (m/s^2)\r\n"
"陀螺仪: X=%s Y=%s Z=%s rad/s\r\n" " Gyro : X=%-10s Y=%-10s Z=%-10s (rad/s)\r\n"
"温度: %s °C\r\n" " Euler : Roll=%-8s Pitch=%-8s Yaw=%-8s (deg)\r\n"
"欧拉角: Roll=%s Pitch=%s Yaw=%s °\r\n", " Quaternion: q0=%-10s q1=%-10s q2=%-10s q3=%-10s\r\n"
imu->header.online ? "在线" : "离线", " Temp : %-10s C\r\n",
imu->header.online ? "Online" : "Offline",
ax, ay, az, ax, ay, az,
gx, gy, gz, gx, gy, gz,
temp, roll, pitch, yaw,
roll, pitch, yaw); q0, q1, q2, q3,
temp);
} }
/** /**
@ -270,14 +286,14 @@ static int print_motor_device(const void *device_data, char *buffer, size_t buff
format_float(temp, sizeof(temp), motor->feedback.temp, 1); format_float(temp, sizeof(temp), motor->feedback.temp, 1);
return snprintf(buffer, buffer_size, return snprintf(buffer, buffer_size,
"状态: %s\r\n" " Status : %s\r\n"
"反装: %s\r\n" " Reverse : %s\r\n"
"角度: %s °\r\n" " Angle : %-15s deg\r\n"
"转速: %s RPM\r\n" " Speed : %-15s RPM\r\n"
"电流: %s A\r\n" " Current : %-15s A\r\n"
"温度: %s °C\r\n", " Temp : %-15s C\r\n",
motor->header.online ? "在线" : "离线", motor->header.online ? "Online" : "Offline",
motor->reverse ? "是" : "否", motor->reverse ? "Yes" : "No",
angle, speed, current, temp); angle, speed, current, temp);
} }
@ -294,7 +310,7 @@ static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const ch
int offset = snprintf(pcWriteBuffer, xWriteBufferLen, int offset = snprintf(pcWriteBuffer, xWriteBufferLen,
"MRobot CLI v2.0\r\n" "MRobot CLI v2.0\r\n"
"================\r\n" "================\r\n"
"内置命令:\r\n"); "Built-in Commands:\r\n");
for (size_t i = 0; i < BUILTIN_CMD_COUNT && offset < (int)xWriteBufferLen - 50; i++) { for (size_t i = 0; i < BUILTIN_CMD_COUNT && offset < (int)xWriteBufferLen - 50; i++) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
@ -303,7 +319,7 @@ static BaseType_t cmd_help(char *pcWriteBuffer, size_t xWriteBufferLen, const ch
if (ctx.custom_cmd_count > 0) { if (ctx.custom_cmd_count > 0) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
"\r\n自定义命令:\r\n"); "\r\nCustom Commands:\r\n");
for (uint8_t i = 0; i < ctx.custom_cmd_count && offset < (int)xWriteBufferLen - 50; i++) { for (uint8_t i = 0; i < ctx.custom_cmd_count && offset < (int)xWriteBufferLen - 50; i++) {
if (ctx.custom_cmds[i] != NULL) { if (ctx.custom_cmds[i] != NULL) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
@ -347,7 +363,7 @@ static BaseType_t cmd_cd(char *pcWriteBuffer, size_t xWriteBufferLen, const char
if (param == NULL) { if (param == NULL) {
/* 无参数时切换到根目录 */ /* 无参数时切换到根目录 */
strcpy(ctx.current_path, "/"); strcpy(ctx.current_path, "/");
snprintf(pcWriteBuffer, xWriteBufferLen, "切换到: %s\r\n", ctx.current_path); snprintf(pcWriteBuffer, xWriteBufferLen, "Changed to: %s\r\n", ctx.current_path);
return pdFALSE; return pdFALSE;
} }
@ -365,11 +381,11 @@ static BaseType_t cmd_cd(char *pcWriteBuffer, size_t xWriteBufferLen, const char
} else if (strcmp(path, "modules") == 0 || strcmp(path, "/modules") == 0) { } else if (strcmp(path, "modules") == 0 || strcmp(path, "/modules") == 0) {
strcpy(ctx.current_path, "/modules"); strcpy(ctx.current_path, "/modules");
} else { } else {
snprintf(pcWriteBuffer, xWriteBufferLen, "错误: 目录 '%s' 不存在\r\n", path); snprintf(pcWriteBuffer, xWriteBufferLen, "Error: Directory '%s' not found\r\n", path);
return pdFALSE; return pdFALSE;
} }
snprintf(pcWriteBuffer, xWriteBufferLen, "切换到: %s\r\n", ctx.current_path); snprintf(pcWriteBuffer, xWriteBufferLen, "Changed to: %s\r\n", ctx.current_path);
return pdFALSE; return pdFALSE;
} }
@ -385,30 +401,35 @@ static BaseType_t cmd_ls(char *pcWriteBuffer, size_t xWriteBufferLen, const char
"dev/\r\n" "dev/\r\n"
"modules/\r\n"); "modules/\r\n");
} else if (strcmp(ctx.current_path, "/dev") == 0) { } else if (strcmp(ctx.current_path, "/dev") == 0) {
offset = snprintf(pcWriteBuffer, xWriteBufferLen, "设备列表 (%d 个):\r\n", ctx.device_count); offset = snprintf(pcWriteBuffer, xWriteBufferLen,
"Device List (%d devices)\r\n\r\n",
ctx.device_count);
if (ctx.device_count == 0) { if (ctx.device_count == 0) {
snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, " (无设备)\r\n"); offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" (No devices)\r\n");
} else { } else {
/* 按类型分组显示 */ /* 按类型分组显示 */
static const char *type_names[] = { "IMU", "电机", "传感器", "自定义" }; static const char *type_names[] = { "IMU", "Motor", "Sensor", "Custom" };
for (uint8_t t = 0; t < MROBOT_DEVICE_TYPE_NUM && offset < (int)xWriteBufferLen - 50; t++) { for (uint8_t t = 0; t < MROBOT_DEVICE_TYPE_NUM && offset < (int)xWriteBufferLen - 50; t++) {
bool has_type = false; bool has_type = false;
for (uint8_t i = 0; i < ctx.device_count; i++) { for (uint8_t i = 0; i < ctx.device_count; i++) {
if (ctx.devices[i].type == t) { if (ctx.devices[i].type == t) {
if (!has_type) { if (!has_type) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" [%s]\r\n", type_names[t]); "\r\n[%s]\r\n", type_names[t]);
has_type = true; has_type = true;
} }
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
" %s\r\n", ctx.devices[i].name); " - %s\r\n", ctx.devices[i].name);
} }
} }
} }
} }
} else if (strcmp(ctx.current_path, "/modules") == 0) { } else if (strcmp(ctx.current_path, "/modules") == 0) {
snprintf(pcWriteBuffer, xWriteBufferLen, "(模块功能暂未实现)\r\n"); snprintf(pcWriteBuffer, xWriteBufferLen,
"Module functions not yet implemented\r\n");
} }
return pdFALSE; return pdFALSE;
@ -432,8 +453,8 @@ static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const ch
/* 检查是否在 /dev 目录 */ /* 检查是否在 /dev 目录 */
if (strcmp(ctx.current_path, "/dev") != 0) { if (strcmp(ctx.current_path, "/dev") != 0) {
snprintf(pcWriteBuffer, xWriteBufferLen, snprintf(pcWriteBuffer, xWriteBufferLen,
"错误: show 命令仅在 /dev 目录下可用\r\n" "Error: 'show' command only works in /dev directory\r\n"
"提示: 使用 'cd /dev' 切换到设备目录\r\n"); "Hint: Use 'cd /dev' to switch to device directory\r\n");
return pdFALSE; return pdFALSE;
} }
@ -478,7 +499,7 @@ static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const ch
if (target_device[0] == '\0') { if (target_device[0] == '\0') {
/* 显示所有设备 */ /* 显示所有设备 */
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
"=== 所有设备 ===\r\n\r\n"); "=== All Devices ===\r\n\r\n");
for (uint8_t i = 0; i < ctx.device_count && offset < (int)xWriteBufferLen - 100; i++) { for (uint8_t i = 0; i < ctx.device_count && offset < (int)xWriteBufferLen - 100; i++) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
@ -491,14 +512,13 @@ static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const ch
offset += (written > 0) ? written : 0; offset += (written > 0) ? written : 0;
} else { } else {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
"(无打印函数)\r\n"); " (No print function)\r\n");
} }
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, "\r\n");
} }
if (ctx.device_count == 0) { if (ctx.device_count == 0) {
offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, offset += snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
"(无已注册设备)\r\n"); " (No devices registered)\r\n");
} }
} else { } else {
/* 显示指定设备 */ /* 显示指定设备 */
@ -506,7 +526,8 @@ static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const ch
if (dev == NULL) { if (dev == NULL) {
snprintf(pcWriteBuffer, xWriteBufferLen, snprintf(pcWriteBuffer, xWriteBufferLen,
"错误: 设备 '%s' 未找到\r\n", target_device); "Error: Device '%s' not found\r\n",
target_device);
current_iter = 0; current_iter = 0;
return pdFALSE; return pdFALSE;
} }
@ -518,7 +539,7 @@ static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const ch
dev->print_cb(dev->data, pcWriteBuffer + offset, xWriteBufferLen - offset); dev->print_cb(dev->data, pcWriteBuffer + offset, xWriteBufferLen - offset);
} else { } else {
snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset, snprintf(pcWriteBuffer + offset, xWriteBufferLen - offset,
"(无打印函数)\r\n"); " (No print function)\r\n");
} }
} }
@ -533,23 +554,12 @@ static BaseType_t cmd_show(char *pcWriteBuffer, size_t xWriteBufferLen, const ch
} }
} }
/* ========================================================================== */ /* ============================================================================
/* htop 模式实现 */ * htop 模式处理
/* ========================================================================== */ * ========================================================================== */
/**
* @brief 处理 htop 模式显示
*/
static void handle_htop_mode(void) { static void handle_htop_mode(void) {
/* 清屏 */
send_string(ANSI_CLEAR_SCREEN); send_string(ANSI_CLEAR_SCREEN);
send_string("=== MRobot Task Monitor (Press 'q' to exit) ===\r\n\r\n");
/* 显示头部 */
send_string(
"MRobot Task Monitor (按 'q' 退出)\r\n"
"================================================================================\r\n"
"Task Name State Prio Stack Num\r\n"
"--------------------------------------------------------------------------------\r\n");
/* 获取任务列表 */ /* 获取任务列表 */
char task_buffer[1024]; char task_buffer[1024];
@ -557,6 +567,10 @@ static void handle_htop_mode(void) {
vTaskList(task_buffer); vTaskList(task_buffer);
/* 表头 */
send_string("Task Name State Prio Stack Num\r\n");
send_string("------------------------------------------------\r\n");
/* 解析并格式化任务列表 */ /* 解析并格式化任务列表 */
char *line = strtok(task_buffer, "\r\n"); char *line = strtok(task_buffer, "\r\n");
while (line != NULL) { while (line != NULL) {
@ -571,7 +585,7 @@ static void handle_htop_mode(void) {
case 'B': state_str = "Blocked"; break; case 'B': state_str = "Blocked"; break;
case 'S': state_str = "Suspend"; break; case 'S': state_str = "Suspend"; break;
case 'D': state_str = "Deleted"; break; case 'D': state_str = "Deleted"; break;
case 'X': state_str = "Ready"; break; case 'X': state_str = "Ready "; break;
default: state_str = "Unknown"; break; default: state_str = "Unknown"; break;
} }
@ -585,7 +599,7 @@ static void handle_htop_mode(void) {
/* 显示系统信息 */ /* 显示系统信息 */
snprintf(display_line, sizeof(display_line), snprintf(display_line, sizeof(display_line),
"--------------------------------------------------------------------------------\r\n" "------------------------------------------------\r\n"
"System Tick: %lu | Free Heap: %lu bytes\r\n", "System Tick: %lu | Free Heap: %lu bytes\r\n",
(unsigned long)xTaskGetTickCount(), (unsigned long)xTaskGetTickCount(),
(unsigned long)xPortGetFreeHeapSize()); (unsigned long)xPortGetFreeHeapSize());