diff --git a/libraries/AP_Menu/AP_Menu.cpp b/libraries/AP_Menu/AP_Menu.cpp index 160a422aef..d95f352537 100644 --- a/libraries/AP_Menu/AP_Menu.cpp +++ b/libraries/AP_Menu/AP_Menu.cpp @@ -37,16 +37,129 @@ Menu::Menu(const prog_char *prompt, const Menu::command *commands, uint8_t entri _argv = NULL; } +/** + check for another input byte on the port and accumulate + return true if we have a full line ready to process + */ +bool +Menu::_check_for_input(void) +{ + if (_port->available() <= 0) { + return false; + } + + // loop reading characters from the input + int c = _port->read(); + + // carriage return -> process command + if ('\r' == c || '\n' == c) { + _inbuf[_input_len] = '\0'; + _port->write('\r'); + _port->write('\n'); + // we have a full line to process + return true; + } + + // backspace + if ('\b' == c) { + if (_input_len > 0) { + _input_len--; + _port->write('\b'); + _port->write(' '); + _port->write('\b'); + return false; + } + } + + // printable character + if (isprint(c) && (_input_len < (_commandline_max - 1))) { + _inbuf[_input_len++] = c; + _port->write((char)c); + } + + return false; +} + + +// run the menu +bool +Menu::_run_command(void) +{ + int8_t ret; + uint8_t i; + uint8_t argc; + char *s = NULL; + + _input_len = 0; + + // split the input line into tokens + argc = 0; + s = NULL; + _argv[argc++].str = strtok_r(_inbuf, " ", &s); + + // XXX should an empty line by itself back out of the current menu? + while (argc <= _args_max) { + _argv[argc].str = strtok_r(NULL, " ", &s); + if ('\0' == _argv[argc].str) + break; + _argv[argc].i = atol(_argv[argc].str); + _argv[argc].f = atof(_argv[argc].str); // calls strtod, > 700B ! + argc++; + } + + if (_argv[0].str == NULL) { + // we got a blank line, re-display the prompt + _port->printf_P(PSTR("%S] "), FPSTR(_prompt)); + return false; + } + + // populate arguments that have not been specified with "" and 0 + // this is safer than NULL in the case where commands may look + // without testing argc + i = argc; + while (i <= _args_max) { + _argv[i].str = ""; + _argv[i].i = 0; + _argv[i].f = 0; + i++; + } + + bool cmd_found = false; + // look for a command matching the first word (note that it may be empty) + for (i = 0; i < _entries; i++) { + if (!strcasecmp_P(_argv[0].str, _commands[i].command)) { + ret = _call(i, argc); + cmd_found=true; + if (-2 == ret) + return true; + break; + } + } + + // implicit commands + if (i == _entries) { + if (!strcmp(_argv[0].str, "?") || (!strcasecmp_P(_argv[0].str, PSTR("help")))) { + _help(); + cmd_found=true; + } else if (!strcasecmp_P(_argv[0].str, PSTR("exit"))) { + // exit the menu + return true; + } + } + + if (cmd_found==false) + { + _port->println_P(PSTR("Invalid command, type 'help'")); + } + + return false; +} + + // run the menu void Menu::run(void) { - int8_t ret; - uint8_t len, i; - uint8_t argc; - int c; - char *s = NULL; - if (_port == NULL) { // default to main serial port _port = hal.console; @@ -62,96 +175,37 @@ Menu::run(void) return; // loop reading characters from the input - len = 0; + _input_len = 0; _port->printf_P(PSTR("%S] "), FPSTR(_prompt)); + for (;; ) { - c = _port->read(); - if (-1 == c) { - hal.scheduler->delay(20); - continue; - } - // carriage return -> process command - if ('\r' == c || '\n' == c) { - _inbuf[len] = '\0'; - _port->write('\r'); - _port->write('\n'); + if (_check_for_input()) { break; } - // backspace - if ('\b' == c) { - if (len > 0) { - len--; - _port->write('\b'); - _port->write(' '); - _port->write('\b'); - continue; - } - } - // printable character - if (isprint(c) && (len < (MENU_COMMANDLINE_MAX - 1))) { - _inbuf[len++] = c; - _port->write((char)c); - continue; - } + hal.scheduler->delay(20); } - // split the input line into tokens - argc = 0; - s = NULL; - _argv[argc++].str = strtok_r(_inbuf, " ", &s); - // XXX should an empty line by itself back out of the current menu? - while (argc <= MENU_ARGS_MAX) { - _argv[argc].str = strtok_r(NULL, " ", &s); - if ('\0' == _argv[argc].str) - break; - _argv[argc].i = atol(_argv[argc].str); - _argv[argc].f = atof(_argv[argc].str); // calls strtod, > 700B ! - argc++; - } + // we have a full command to run + if (_run_command()) break; + } +} - if (_argv[0].str == NULL) { - continue; - } +// check for new user input +bool +Menu::check_input(void) +{ + if (_port == NULL) { + // default to main serial port + _port = hal.console; + } - // populate arguments that have not been specified with "" and 0 - // this is safer than NULL in the case where commands may look - // without testing argc - i = argc; - while (i <= MENU_ARGS_MAX) { - _argv[i].str = ""; - _argv[i].i = 0; - _argv[i].f = 0; - i++; - } + _allocate_buffers(); - bool cmd_found = false; - // look for a command matching the first word (note that it may be empty) - for (i = 0; i < _entries; i++) { - if (!strcasecmp_P(_argv[0].str, _commands[i].command)) { - ret = _call(i, argc); - cmd_found=true; - if (-2 == ret) - return; - break; - } - } + if (_check_for_input()) { + return _run_command(); + } - // implicit commands - if (i == _entries) { - if (!strcmp(_argv[0].str, "?") || (!strcasecmp_P(_argv[0].str, PSTR("help")))) { - _help(); - cmd_found=true; - } else if (!strcasecmp_P(_argv[0].str, PSTR("exit"))) { - return; - } - } - - if (cmd_found==false) - { - _port->println_P(PSTR("Invalid command, type 'help'")); - } - - } + return false; } // display the list of commands in response to the 'help' command diff --git a/libraries/AP_Menu/AP_Menu.h b/libraries/AP_Menu/AP_Menu.h index 7055b104e1..19f2b37b15 100644 --- a/libraries/AP_Menu/AP_Menu.h +++ b/libraries/AP_Menu/AP_Menu.h @@ -109,6 +109,11 @@ public: /// menu runner void run(void); + /// check for new input on the port. Can be used + /// to allow for the menu to operate asynchronously + /// this will return true if the user asked to exit the menu + bool check_input(void); + private: /// Implements the default 'help' command. /// @@ -135,6 +140,16 @@ private: // allocate inbuf and args buffers void _allocate_buffers(void); + // number of characters read so far from the port + uint8_t _input_len; + + // check for next input character + bool _check_for_input(void); + + // run one full entered command. + // return true if the menu loop should exit + bool _run_command(void); + // port to run on static AP_HAL::BetterStream *_port; };