static BOOL check_cmd(const char *cmds, const char *cmd)
{
int start, end;
for (start = end = 0; cmds[end] != 0 && cmds[end] != '|'; end++);
while (end > start) {
if (match_cmd(cmds, start, end - 1, cmd) == TRUE)
return TRUE;
if (cmds[end] == 0)
break;
if (cmds[end] == '|') {
end++;
for (start = end; cmds[end] != 0 && cmds[end] != '|'; end++);
}
}
return FALSE;
}
/* main entry point ... */
unsigned int simple_vsnprintf(char *buf, unsigned int sz,
const char *fmt, va_list ap)
{
unsigned int ret = 0;
while (*fmt)
{
unsigned int wid = 0;
int prec = -1;
int flg_zero = 0;
int flg_hash = 0;
int flg_minus = 0;
int flg_plus = 0;
int flg_spac = 0;
int flg_parse = 1;
if ((*fmt != '%') || (*++fmt == '%'))
{
ADD_C(*fmt++);
continue;
}
/* write out how much data has been written */
FMT_RET_N("hh", signed char);
FMT_RET_N("h", short);
FMT_RET_N("", int);
FMT_RET_N("l", long);
FMT_RET_N("ll", long long);
FMT_RET_N("z", ssize_t);
FMT_RET_N("t", ptrdiff_t);
FMT_RET_N("j", intmax_t);
/* parse the flags for the data */
while (*fmt && flg_parse)
switch (*fmt)
{
case '0':
flg_zero = 1;
++fmt;
break;
case '#':
flg_hash = 1;
++fmt;
break;
case '-':
flg_minus = 1;
++fmt;
break;
case '+':
flg_plus = 1;
++fmt;
break;
case ' ':
flg_spac = 1;
++fmt;
break;
default:
flg_parse = 0;
break;
}
if (match_cmd(&fmt, "*"))
{
int tmp = va_arg(ap, int);
wid = tmp;
if (tmp < 0) wid = -wid;
}
else
static bool_e readline(void)
{
uint8_t c;
uint8_t *p = console_buffer;
int32_t n = 0;
int32_t n_pos = 0;
int32_t ret;
// 打印提示符
printf(prompt);
//fflush(stdout);
while (TRUE)
{
taskDelay(1);
// 检测输入
//if ((c = bsp_getchar()) == 0)
if(ttyRead(consoleFd, &c, 1) == 0)
continue;
// 处理输入字符
switch (c)
{
case '\r': // 回车
case '\n': // 换行，命令结束输入
p[n] = '\0';
printf ("\r\n");
pmatch_cmd = NULL;
return TRUE;
case 0x03: // Ctrl + C
// 清空缓冲区
console_buffer[0] = '\0';
printf("\r\n");
return FALSE;
case 0x08: //退格键
if (0 >= n)
continue;
// 删除最后一个字符
p[n] = '\0';
//p--;
n--;
n_pos = n;
pmatch_cmd = NULL;
printf(erase_seq);
continue;
case 0x09: // TAB
ret = match_cmd((char_t *)console_buffer, n_pos);
n = ret > 0 ? ret : n;
printf("\r");
printf(prompt);
printf("%s", console_buffer);
printf("\033[0K"); // Clear from cursor to end of line
continue;
case 0x7F: // DEL
continue;
default: // 可接受的字符
// Buffer full
if (n >= CFG_CBSIZE - 2)
{
printf("\a");//超出命令最大字节数响铃
continue;
}
printf("%c", c);
// 保存到缓冲区
p[n] = c;
n++;
n_pos = n;
break;
}
}
return TRUE;
}
/**
******************************************************************************
* @brief ¶ÁÈ¡Ò»ÐÐÃüÁî
* @param[in] None
* @param[out] None
* @retval TRUE : ³É¹¦
* @retval FALSE : ʧ°Ü
*
* @details
*
* @note
******************************************************************************
*/
static bool_e
readline(void)
{
uint8_t c;
uint8_t *p = console_buffer;
int32_t n = 0;
int32_t n_pos = 0;
int32_t ret;
// ´òÓ¡Ìáʾ·û
SHELL_PRINTF(prompt);
//fflush(stdout);
while (TRUE)
{
taskDelay(1);
// ¼ì²âÊäÈë
if ((c = bsp_getchar()) == 0)
//if (ttyRead(consoleFd, &c, 1) == 0) todo
{
continue;
}
// ´¦ÀíÊäÈë×Ö·û
switch (c)
{
case '\r': // »Ø³µ
case '\n': // »»ÐУ¬ÃüÁî½áÊøÊäÈë
p[n] = '\0';
SHELL_PRINTF ("\r\n");
pmatch_cmd = NULL;
return TRUE;
case 0x03: // Ctrl + C
// Çå¿Õ»º³åÇø
console_buffer[0] = '\0';
SHELL_PRINTF("\r\n");
return FALSE;
case 0x08: //Í˸ñ¼ü
if (0 >= n)
{
continue;
}
// ɾ³ý×îºóÒ»¸ö×Ö·û
p[n] = '\0';
//p--;
n--;
n_pos = n;
pmatch_cmd = NULL;
SHELL_PRINTF(erase_seq);
continue;
case 0x09: // TAB
ret = match_cmd((char_t *)console_buffer, n_pos);
n = ret > 0 ? ret : n;
SHELL_PRINTF("\r");
SHELL_PRINTF(prompt);
SHELL_PRINTF("%s", console_buffer);
SHELL_PRINTF("\033[0K"); // Clear from cursor to end of line
continue;
case 0x7F: // DEL
continue;
default: // ¿É½ÓÊܵÄ×Ö·û
// Buffer full
if (n >= CFG_CBSIZE - 2)
{
SHELL_PRINTF("\a");//³¬³öÃüÁî×î´ó×Ö½ÚÊýÏìÁå
continue;
}
SHELL_PRINTF("%c", c);
// ±£´æµ½»º³åÇø
p[n] = c;
n++;
n_pos = n;
break;
}
}
return TRUE;
}
void mavlink_message_handler_receive(mavlink_message_handler_t* message_handler, mavlink_received_t* rec)
{
mavlink_message_t* msg = &rec->msg;
if ( message_handler->debug )
{
mavlink_message_handler_msg_default_dbg(msg);
}
if (msg->msgid == MAVLINK_MSG_ID_COMMAND_LONG)
{
// The message is a command
mavlink_command_long_t cmd;
mavlink_msg_command_long_decode(msg, &cmd);
//print packet command and parameters for debug
print_util_dbg_print("target sysID:");
print_util_dbg_print_num(cmd.target_system,10);
print_util_dbg_print(", target compID:");
print_util_dbg_print_num(cmd.target_component,10);
print_util_dbg_print("\r\n");
print_util_dbg_print("parameters: ");
print_util_dbg_print_num(cmd.param1,10);
print_util_dbg_print_num(cmd.param2,10);
print_util_dbg_print_num(cmd.param3,10);
print_util_dbg_print_num(cmd.param4,10);
print_util_dbg_print_num(cmd.param5,10);
print_util_dbg_print_num(cmd.param6,10);
print_util_dbg_print_num(cmd.param7,10);
print_util_dbg_print("\r\n");
print_util_dbg_print("command id:");
print_util_dbg_print_num(cmd.command,10);
print_util_dbg_print(", confirmation:");
print_util_dbg_print_num(cmd.confirmation,10);
print_util_dbg_print("\r\n");
if (cmd.command >= 0 && cmd.command < MAV_CMD_ENUM_END)
{
// The command has valid command ID
if( (cmd.target_system == message_handler->mavlink_stream->sysid)||(cmd.target_system == MAV_SYS_ID_ALL) )
{
mav_result_t result = MAV_RESULT_UNSUPPORTED;
// The command is for this system
for (uint32_t i = 0; i < message_handler->cmd_callback_set->callback_count; ++i)
{
if ( match_cmd(message_handler, &message_handler->cmd_callback_set->callback_list[i], msg, &cmd) )
{
mavlink_cmd_callback_function_t function = message_handler->cmd_callback_set->callback_list[i].function;
handling_module_struct_t module_struct = message_handler->cmd_callback_set->callback_list[i].module_struct;
// Call appropriate function callback
result = function(module_struct, &cmd);
break;
}
}
// Send acknowledgment message
mavlink_message_t msg;
mavlink_msg_command_ack_pack( message_handler->mavlink_stream->sysid,
message_handler->mavlink_stream->compid,
&msg,
cmd.command,
result);
mavlink_stream_send(message_handler->mavlink_stream, &msg);
}
}
}
else if ( msg->msgid >= 0 && msg->msgid < MAV_MSG_ENUM_END )
{
// The message has a valid message ID, and is not a command
for (uint32_t i = 0; i < message_handler->msg_callback_set->callback_count; ++i)
{
if ( match_msg(message_handler, &message_handler->msg_callback_set->callback_list[i], msg) )
{
mavlink_msg_callback_function_t function = message_handler->msg_callback_set->callback_list[i].function;
handling_module_struct_t module_struct = message_handler->msg_callback_set->callback_list[i].module_struct;
uint32_t sys_id = *message_handler->msg_callback_set->callback_list[i].sys_id;
// Call appropriate function callback
function(module_struct, sys_id, msg);
}
}
}
}
