RMUL2025/User/task/cli.c
2025-03-09 17:01:52 +08:00

745 lines
23 KiB
C
Raw Blame History

This file contains ambiguous Unicode characters

This file contains Unicode characters that might be confused with other characters. If you think that this is intentional, you can safely ignore this warning. Use the Escape button to reveal them.

/*
命令行交互界面Command Line Interface任务
实现命令行。
从USB虚拟串口读取数据结果也打印到USB虚拟串口。
*/
/* Includes ----------------------------------------------------------------- */
#include <stdbool.h>
#include <stdint.h>
#include <stdio.h>
#include <string.h>
#include "FreeRTOS.h"
#include "bsp\can.h"
#include "bsp\usb.h"
#include "component\FreeRTOS_CLI.h"
#include "task.h"
#include "task\user_task.h"
/* Private typedef ---------------------------------------------------------- */
typedef struct {
uint8_t stage;
} FiniteStateMachine_t;
/* Private define ----------------------------------------------------------- */
#define MAX_INPUT_LENGTH 64
/* Private macro ------------------------------------------------------------ */
/* Private variables -------------------------------------------------------- */
static const char *const CLI_WELCOME_MESSAGE =
"\r\n"
// " ______ __ _______ __ \r\n"
// " | __ \\.-----.| |--.-----.| | |.---.-.-----.| |_.-----.----.\r\n"
// " | <| _ || _ | _ || || _ |__ --|| _| -__| _|\r\n"
// " |___|__||_____||_____|_____||__|_|__||___._|_____||____|_____|__| \r\n"
" -------------------------------------------------------------------\r\n"
" FreeRTOS CLI. Type 'help' to view a list of registered commands. \r\n"
"\r\n";
/* Command示例 */
static BaseType_t Command_Endian(char *out_buffer, size_t len,
const char *command_string) {
if (out_buffer == NULL) return pdFALSE;
(void)command_string; /* 没用到command_string消除警告 */
len -= 1; /* 字符串后面有\0 */
/* 任务本身相关的内容 */
uint8_t list[2] = {0x11, 0x22};
uint16_t force_convert = ((uint16_t *)list)[0];
uint16_t assembled = (uint16_t)(list[0] | (list[1] << 8));
static FiniteStateMachine_t fsm; /* 有限状态机 */
switch (fsm.stage) {
case 0:
/* 每个状态内只允许有一个snprintf相关函数以保证安全 */
/* 每个snprintf相关函数必须带有长度限制 */
snprintf(out_buffer, len, "a[2] = {0x11, 0x22}\r\n");
fsm.stage++; /* 改变状态机运行状态 */
return pdPASS; /* 需要继续运行下一状态时返回pdPASS */
case 1:
snprintf(out_buffer, len,
"Force convert to uint16 list, we got: 0x%x\r\n", force_convert);
fsm.stage++;
return pdPASS;
case 2:
snprintf(out_buffer, len,
"Manually assemble a[1], a[0], we got: 0x%x\r\n", assembled);
fsm.stage++;
return pdPASS;
case 3:
if (force_convert == assembled)
snprintf(out_buffer, len, "Small endian\r\n");
else
snprintf(out_buffer, len, "Big endian\r\n");
fsm.stage++;
return pdPASS;
default: /* 结束时状态 */
snprintf(out_buffer, len, "\r\n");
fsm.stage = 0; /* 重置有限状态机 */
return pdFALSE; /* 不需要继续运行下一状态时返回pdFALSE */
}
}
static BaseType_t Command_Stats(char *out_buffer, size_t len,
const char *command_string) {
static const char *const task_list_header =
"\r\n"
"-------------------Task list--------------------\r\n"
"Task State Priority Stack\t#\r\n"
"------------------------------------------------\r\n";
static const char *const run_time_header =
"\r\n"
"-----------------Run time stats-----------------\r\n"
"Task Abs Time Time\r\n"
"------------------------------------------------\r\n";
static const char *const heap_header =
"\r\n"
"-------------------Heap stats-------------------\r\n"
"total(B)\tfree(B)\tused(B)\r\n"
"------------------------------------------------\r\n";
static const char *const hardware_header =
"\r\n"
"-----------------Hardware stats-----------------\r\n"
"CPU temp(C)\tBettary(V)\r\n"
"------------------------------------------------\r\n";
static const char *const config_header =
"\r\n"
"------------------Config stats------------------\r\n"
"Robot param\tPilot config\r\n"
"------------------------------------------------\r\n";
if (out_buffer == NULL) return pdFALSE;
(void)command_string;
len -= 1;
/* 堆区信息的相关内容 */
HeapStats_t heap_stats;
static FiniteStateMachine_t fsm;
switch (fsm.stage) {
case 0:
strncpy(out_buffer, task_list_header, len);
fsm.stage++;
return pdPASS;
case 1:
/* 获取任务的列表 */
vTaskList(out_buffer);
fsm.stage++;
return pdPASS;
case 2:
strncpy(out_buffer, run_time_header, len);
fsm.stage++;
return pdPASS;
case 3:
/* 获得任务的运行时间 */
vTaskGetRunTimeStats(out_buffer);
fsm.stage++;
return pdPASS;
case 4:
strncpy(out_buffer, heap_header, len);
fsm.stage++;
return pdPASS;
case 5:
/* 获得堆区的可用空间 */
vPortGetHeapStats(&heap_stats);
snprintf(out_buffer, len, "%d\t\t%d\t%d\r\n", configTOTAL_HEAP_SIZE,
heap_stats.xAvailableHeapSpaceInBytes,
configTOTAL_HEAP_SIZE - heap_stats.xAvailableHeapSpaceInBytes);
fsm.stage++;
return pdPASS;
case 6:
strncpy(out_buffer, hardware_header, len);
fsm.stage++;
return pdPASS;
case 7:
/* 获取当前CPU温度和电量 */
snprintf(out_buffer, len, "%f\t%f\r\n", task_runtime.status.cpu_temp,
task_runtime.status.battery);
fsm.stage++;
return pdPASS;
case 8:
strncpy(out_buffer, config_header, len);
fsm.stage++;
return pdPASS;
case 9:
/* 获取机器人和操作手名称 */
snprintf(out_buffer, len, "%s\t\t%s\r\n",
task_runtime.cfg.robot_param_name,
task_runtime.cfg.pilot_cfg_name);
fsm.stage++;
return pdPASS;
default:
snprintf(out_buffer, len, "\r\n");
fsm.stage = 0;
return pdFALSE;
}
}
static BaseType_t Command_Config(char *out_buffer, size_t len,
const char *command_string) {
/* 帮助信息const保证不占用内存空间 */
static const char *const help_string =
"\r\n"
"usage: config <command> [<args>]\r\n"
"These are commands:\r\n"
" help Display this message\r\n"
" init Init config after flashing\r\n"
" list <pilot/robot> List available config\r\n"
" set <pilot/robot> <name> Set config\r\n"
"\r\n";
/* 用常量表示状态机每个阶段,比数字更清楚 */
static const uint8_t stage_begin = 0;
static const uint8_t stage_success = 1;
static const uint8_t stage_end = 2;
if (out_buffer == NULL) return pdFALSE;
len -= 1;
/* 获取每一段参数的开始地址和长度 */
BaseType_t command_len, pr_len, name_len;
const char *command =
FreeRTOS_CLIGetParameter(command_string, 1, &command_len);
const char *pr = FreeRTOS_CLIGetParameter(command_string, 2, &pr_len);
const char *name = FreeRTOS_CLIGetParameter(command_string, 3, &name_len);
Config_t cfg;
static FiniteStateMachine_t fsm;
if (strncmp(command, "help", command_len) == 0) {
/* config help */
snprintf(out_buffer, len, "%s", help_string);
fsm.stage = stage_begin;
return pdFALSE;
} else if (strncmp(command, "init", command_len) == 0) {
if ((pr != NULL) || (name != NULL)) goto command_error;
/* config init */
switch (fsm.stage) {
case stage_begin:
snprintf(out_buffer, len, "\r\nSet Robot config to default and qs.");
fsm.stage = stage_success;
return pdPASS;
case stage_success:
/* 初始化获取的操作手配置、机器人参数 */
memset(&cfg, 0, sizeof(cfg));
cfg.pilot_cfg = Config_GetPilotCfg("qs");
cfg.robot_param = Config_GetRobotParam("default");
snprintf(cfg.robot_param_name, 20, "default");
snprintf(cfg.pilot_cfg_name, 20, "qs");
Config_Set(&cfg);
snprintf(out_buffer, len, "\r\nDone.");
fsm.stage = stage_end;
return pdPASS;
}
} else if (strncmp(command, "list", command_len) == 0) {
if ((pr == NULL) || (name != NULL)) goto command_error;
/* config list */
static uint8_t i = 0;
if (strncmp(pr, "pilot", pr_len) == 0) {
/* config list pilot */
const Config_PilotCfgMap_t *pilot = Config_GetPilotNameMap();
switch (fsm.stage) {
case stage_begin:
snprintf(out_buffer, len, "\r\nAvailable pilot cfg:");
fsm.stage = stage_success;
return pdPASS;
case stage_success:
if (pilot[i].name != NULL) {
snprintf(out_buffer, len, "\r\n %s", pilot[i].name);
i++;
fsm.stage = stage_success;
} else {
i = 0;
fsm.stage = stage_end;
}
return pdPASS;
}
} else if (strncmp(pr, "robot", pr_len) == 0) {
/* config list robot */
const Config_RobotParamMap_t *robot = Config_GetRobotNameMap();
switch (fsm.stage) {
case stage_begin:
snprintf(out_buffer, len, "\r\nAvailable robot params:");
fsm.stage = stage_success;
return pdPASS;
case stage_success:
if (robot[i].name != NULL) {
snprintf(out_buffer, len, "\r\n %s", robot[i].name);
i++;
fsm.stage = stage_success;
} else {
i = 0;
fsm.stage = stage_end;
}
return pdPASS;
}
} else {
goto command_error;
}
} else if (strncmp(command, "set", command_len) == 0) {
if ((pr == NULL) || (name == NULL)) goto command_error;
/* config set */
if (strncmp(pr, "robot", pr_len) == 0) {
/* config set robot */
if (fsm.stage == stage_begin) {
Config_Get(&cfg);
if (Config_GetRobotParam(name) == NULL) {
snprintf(out_buffer, len, "\r\nFailed: Unknow model.");
fsm.stage = stage_end;
return pdPASS;
} else {
snprintf(out_buffer, len, "\r\nSucceed.");
snprintf(cfg.robot_param_name, 20, "%s", name);
Config_Set(&cfg);
fsm.stage = stage_success;
return pdPASS;
}
}
} else if (strncmp(pr, "pilot", pr_len) == 0) {
/* config set pilot */
if (fsm.stage == 0) {
Config_Get(&cfg);
if (Config_GetPilotCfg(name) == NULL) {
snprintf(out_buffer, len, "\r\nFailed: Unknow pilot.");
fsm.stage = stage_end;
return pdPASS;
} else {
snprintf(out_buffer, len, "\r\nSucceed.");
snprintf(cfg.pilot_cfg_name, 20, "%s", name);
Config_Set(&cfg);
fsm.stage = stage_success;
return pdPASS;
}
}
} else {
goto command_error;
}
if (fsm.stage == stage_success) {
snprintf(out_buffer, len,
"\r\nRestart needed for setting to take effect.");
fsm.stage = stage_end;
return pdPASS;
}
} /* config set */
if (fsm.stage == stage_end) {
snprintf(out_buffer, len, "\r\n\r\n");
fsm.stage = stage_begin;
return pdFALSE;
}
command_error:
snprintf(out_buffer, len,
"\r\nconfig: invalid command. See 'config help'.\r\n");
fsm.stage = stage_begin;
return pdFALSE;
}
static BaseType_t Command_CaliGyro(char *out_buffer, size_t len,
const char *command_string) {
if (out_buffer == NULL) return pdFALSE;
(void)command_string;
len -= 1;
/* 陀螺仪校准相关内容 */
Config_t cfg;
AHRS_Gyro_t gyro;
uint16_t count = 0;
static float x = 0.0f;
static float y = 0.0f;
static float z = 0.0f;
static uint8_t retry = 0;
static FiniteStateMachine_t fsm;
switch (fsm.stage) {
case 0:
snprintf(out_buffer, len, "\r\nStart gyroscope calibration.\r\n");
fsm.stage++;
return pdPASS;
case 1:
snprintf(out_buffer, len, "Please make the MCU stable.\r\n");
fsm.stage++;
return pdPASS;
case 2:
/* 无陀螺仪数据和校准重试超时时,校准失败 */
osThreadSuspend(task_runtime.thread.ctrl_gimbal);
if (osMessageQueueGet(task_runtime.msgq.gimbal.gyro, &gyro, NULL, 5) !=
osOK) {
snprintf(out_buffer, len, "Can not get gyro data.\r\n");
fsm.stage = 7;
osThreadResume(task_runtime.thread.ctrl_gimbal);
return pdPASS;
}
osThreadResume(task_runtime.thread.ctrl_gimbal);
if (BMI088_GyroStable(&gyro)) {
snprintf(out_buffer, len,
"Controller is stable. Start calibation.\r\n");
fsm.stage++;
return pdPASS;
} else {
retry++;
if (retry < 3) {
snprintf(out_buffer, len, "Controller is unstable.\r\n");
fsm.stage = 1;
} else {
fsm.stage = 7;
}
return pdPASS;
}
case 3:
/* 记录1000组数据求出平均值作为陀螺仪的3轴零偏 */
snprintf(out_buffer, len, "Calibation in progress.\r\n");
Config_Get(&cfg);
cfg.cali.bmi088.gyro_offset.x = 0.0f;
cfg.cali.bmi088.gyro_offset.y = 0.0f;
cfg.cali.bmi088.gyro_offset.z = 0.0f;
Config_Set(&cfg);
while (count < 1000) {
bool data_new = (osMessageQueueGet(task_runtime.msgq.gimbal.gyro, &gyro,
NULL, 5) == osOK);
bool data_good = BMI088_GyroStable(&gyro);
if (data_new && data_good) {
x += gyro.x;
y += gyro.y;
z += gyro.z;
count++;
}
}
x /= (float)count;
y /= (float)count;
z /= (float)count;
fsm.stage++;
return pdPASS;
case 4:
snprintf(out_buffer, len,
"Calibation finished. Write result to flash.\r\n");
Config_Get(&cfg);
cfg.cali.bmi088.gyro_offset.x = x;
cfg.cali.bmi088.gyro_offset.y = y;
cfg.cali.bmi088.gyro_offset.z = z;
Config_Set(&cfg);
fsm.stage++;
return pdPASS;
case 5:
snprintf(out_buffer, len, "x:%.5f; y:%.5f; z:%.5f;\r\n", x, y, z);
fsm.stage++;
return pdPASS;
case 6:
snprintf(out_buffer, len, "Calibation done.\r\n");
fsm.stage = 8;
return pdPASS;
case 7:
snprintf(out_buffer, len, "Calibation failed.\r\n");
fsm.stage = 8;
return pdPASS;
default:
x = 0.0f;
y = 0.0f;
z = 0.0f;
retry = 0;
snprintf(out_buffer, len, "\r\n");
fsm.stage = 0;
return pdFALSE;
}
}
static BaseType_t Command_SetMechZero(char *out_buffer, size_t len,
const char *command_string) {
if (out_buffer == NULL) return pdFALSE;
(void)command_string;
len -= 1;
CAN_t can;
Config_t cfg;
static FiniteStateMachine_t fsm;
switch (fsm.stage) {
case 0:
snprintf(out_buffer, len, "\r\nStart setting mechanical zero point.\r\n");
fsm.stage++;
return pdPASS;
case 1:
/* 获取到云台数据用can上的新的云台机械零点的位置替代旧的位置 */
Config_Get(&cfg);
osThreadSuspend(task_runtime.thread.ctrl_gimbal);
if (osMessageQueueGet(task_runtime.msgq.can.feedback.gimbal, &can, NULL,
5) != osOK) {
snprintf(out_buffer, len, "Can not get gimbal data.\r\n");
fsm.stage = 2;
osThreadResume(task_runtime.thread.ctrl_gimbal);
return pdPASS;
}
osThreadResume(task_runtime.thread.ctrl_gimbal);
cfg.mech_zero.yaw = can.motor.gimbal.named.yaw.rotor_angle;
cfg.mech_zero.pit = can.motor.gimbal.named.pit.rotor_angle;
Config_Set(&cfg);
snprintf(out_buffer, len, "yaw:%f, pitch:%f, rol:%f\r\nDone.",
cfg.mech_zero.yaw, cfg.mech_zero.pit, cfg.mech_zero.rol);
fsm.stage = 3;
return pdPASS;
case 2:
snprintf(out_buffer, len, "Calibation failed.\r\n");
fsm.stage = 3;
return pdPASS;
default:
snprintf(out_buffer, len, "\r\n");
fsm.stage = 0;
return pdFALSE;
}
}
static BaseType_t Command_SetGimbalLim(char *out_buffer, size_t len,
const char *command_string) {
if (out_buffer == NULL) return pdFALSE;
(void)command_string;
len -= 1;
CAN_t can;
Config_t cfg;
static FiniteStateMachine_t fsm;
switch (fsm.stage) {
case 0:
Config_Get(&cfg);
snprintf(out_buffer, len, "(old)limit:%f\r\n", cfg.gimbal_limit);
fsm.stage = 1;
return pdPASS;
case 1:
/* 获取云台数据,获取新的限位角并替代旧的限位角 */
osThreadSuspend(task_runtime.thread.ctrl_gimbal);
if (osMessageQueueGet(task_runtime.msgq.can.feedback.gimbal, &can, NULL,
10) != osOK) {
osThreadResume(task_runtime.thread.ctrl_gimbal);
fsm.stage = 3;
return pdPASS;
}
Config_Get(&cfg);
osThreadResume(task_runtime.thread.ctrl_gimbal);
cfg.gimbal_limit = can.motor.gimbal.named.pit.rotor_angle;
Config_Set(&cfg);
Config_Get(&cfg);
snprintf(out_buffer, len, "(new)limit:%f\r\nDone.", cfg.gimbal_limit);
fsm.stage = 2;
return pdPASS;
case 2:
snprintf(out_buffer, len,
"\r\nRestart needed for setting to take effect.");
fsm.stage = 5;
return pdPASS;
case 3:
snprintf(out_buffer, len,
"Calibation failed.\r\nCan not get gimbal data.\r\n");
fsm.stage = 5;
return pdPASS;
case 4:
snprintf(out_buffer, len, "Unknow options.\r\nPlease check help.");
fsm.stage = 5;
return pdPASS;
default:
snprintf(out_buffer, len, "\r\n");
fsm.stage = 0;
return pdFALSE;
}
}
/*
static BaseType_t Command_XXX(char *out_buffer, size_t len,
const char *command_string) {
if (out_buffer == NULL) return pdFALSE;
(void)command_string;
len -= 1;
static FiniteStateMachine_t fsm;
switch (fsm.stage) {
case 0:
snprintf(out_buffer, len, "\r\nXXX.\r\n");
fsm.stage++;
return pdPASS;
case 1:
XXX();
snprintf(out_buffer, len, "\r\nDone.");
fsm.stage++;
return pdPASS;
default:
snprintf(out_buffer, len, "\r\n");
fsm.stage = 0;
return pdFALSE;
}
}
*/
static const CLI_Command_Definition_t command_table[] = {
{
"endian",
"\r\nendian:\r\n Endian experiment.\r\n\r\n",
Command_Endian,
0,
},
{
"stats",
"\r\nstats:\r\n Displays a table showing the state of "
"FreeRTOS\r\n\r\n",
Command_Stats,
0,
},
{
"config",
"\r\nconfig:\r\n See 'config help'. \r\n\r\n",
Command_Config,
-1,
},
{
"cali-gyro",
"\r\ncali-gyro:\r\n Calibrate gyroscope. Remove zero offset.\r\n\r\n",
Command_CaliGyro,
0,
},
{
"set-mech-zero",
"\r\nset-mech-zero:\r\n Set mechanical zero point for gimbal.\r\n\r\n",
Command_SetMechZero,
0,
},
{
"set-gimbal-limit",
"\r\nset-gimbal-limit:\r\n Move the gimbal to the peak and execute this "
"command to calibrate the limit of gimbal.\r\n\r\n",
Command_SetGimbalLim,
0,
},
/*
{
"xxx-xxx",
"\r\nxxx-xxx:\r\n how to use. ",
Command_XXX,
1,
},
*/
};
/* Private function --------------------------------------------------------- */
/* Exported functions ------------------------------------------------------- */
/**
* \brief 命令行交互界面
*
* \param argument 未使用
*/
void Task_CLI(void *argument) {
(void)argument; /* 未使用argument消除警告 */
static char input[MAX_INPUT_LENGTH]; /* 输入字符串缓存 */
char *output = FreeRTOS_CLIGetOutputBuffer(); /* 输出字符串缓存 */
char rx_char; /* 接收到的字符 */
uint16_t index = 0; /* 字符串索引值 */
BaseType_t processing = 0; /* 命令行解析控制 */
/* 注册所有命令 */
const size_t num_commands =
sizeof(command_table) / sizeof(CLI_Command_Definition_t);
for (size_t j = 0; j < num_commands; j++) {
FreeRTOS_CLIRegisterCommand(command_table + j);
}
/* 通过回车键唤醒命令行界面 */
BSP_USB_Printf("Please press ENTER to activate this console.\r\n");
while (1) {
/* 等待接收到新的字符 */
BSP_USB_ReadyReceive(osThreadGetId());
osThreadFlagsWait(SIGNAL_BSP_USB_BUF_RECV, osFlagsWaitAll, osWaitForever);
/* 读取接收到的新字符 */
rx_char = BSP_USB_ReadChar();
/* 进行判断 */
if (rx_char == '\n' || rx_char == '\r') {
BSP_USB_Printf("%c", rx_char);
break;
}
}
/* 打印欢迎信息 */
BSP_USB_Printf(CLI_WELCOME_MESSAGE);
/* 开始运行命令行界面 */
BSP_USB_Printf("rm>");
while (1) {
#ifdef DEBUG
/* 记录任务所使用的的栈空间 */
task_runtime.stack_water_mark.cli = osThreadGetStackSpace(osThreadGetId());
#endif
/* 等待输入. */
BSP_USB_ReadyReceive(osThreadGetId());
osThreadFlagsWait(SIGNAL_BSP_USB_BUF_RECV, osFlagsWaitAll, osWaitForever);
/* 读取接收到的新字符 */
rx_char = BSP_USB_ReadChar();
if (rx_char <= 126 && rx_char >= 32) {
/* 如果字符是可显示字符,则直接显式,并存入输入缓存中 */
if (index < MAX_INPUT_LENGTH) {
BSP_USB_Printf("%c", rx_char);
input[index] = rx_char;
index++;
}
} else {
/* 如果字符是控制字符,则需要进一步判断 */
if (rx_char == '\n' || rx_char == '\r') {
/* 如果输入的是回车,则认为命令输入完毕,进行下一步的解析和运行命令 */
BSP_USB_Printf("\r\n");
if (index > 0) {
/* 只在输入缓存有内容时起效 */
do {
/* 处理命令 */
processing = FreeRTOS_CLIProcessCommand(
input, output, configCOMMAND_INT_MAX_OUTPUT_SIZE);
BSP_USB_Printf(output); /* 打印结果 */
memset(output, 0x00, strlen(output)); /* 清空输出缓存 */
} while (processing != pdFALSE); /* 是否需要重复运行命令 */
index = 0; /* 重置索引,准备接收下一段命令 */
memset(input, 0x00, strlen(input)); /* 清空输入缓存 */
}
BSP_USB_Printf("rm>");
} else if (rx_char == '\b' || rx_char == 0x7Fu) {
/* 如果输入的是退格键则清空一位输入缓存,同时进行界限保护 */
if (index > 0) {
BSP_USB_Printf("%c", rx_char);
index--;
input[index] = 0;
}
}
}
}
}