mirror of https://github.com/ArduPilot/ardupilot
AP_Menu: ported to AP_HAL
This commit is contained in:
parent
ec53515648
commit
3b1150d857
|
@ -4,19 +4,22 @@
|
||||||
// Simple commandline menu system.
|
// Simple commandline menu system.
|
||||||
//
|
//
|
||||||
|
|
||||||
#include <FastSerial.h>
|
|
||||||
#include <AP_Common.h>
|
#include <AP_Common.h>
|
||||||
|
#include <AP_Progmem.h>
|
||||||
|
#include <AP_HAL.h>
|
||||||
|
|
||||||
|
#include <stdlib.h>
|
||||||
#include <ctype.h>
|
#include <ctype.h>
|
||||||
#include <string.h>
|
#include <string.h>
|
||||||
#include <avr/pgmspace.h>
|
|
||||||
|
|
||||||
#include "AP_Menu.h"
|
#include "AP_Menu.h"
|
||||||
|
|
||||||
|
extern const AP_HAL::HAL& hal;
|
||||||
|
|
||||||
// statics
|
// statics
|
||||||
char Menu::_inbuf[MENU_COMMANDLINE_MAX];
|
char Menu::_inbuf[MENU_COMMANDLINE_MAX];
|
||||||
Menu::arg Menu::_argv[MENU_ARGS_MAX + 1];
|
Menu::arg Menu::_argv[MENU_ARGS_MAX + 1];
|
||||||
FastSerial *Menu::_port;
|
AP_HAL::BetterStream *Menu::_port;
|
||||||
|
|
||||||
|
|
||||||
// constructor
|
// constructor
|
||||||
|
@ -36,15 +39,15 @@ Menu::run(void)
|
||||||
uint8_t len, i;
|
uint8_t len, i;
|
||||||
uint8_t argc;
|
uint8_t argc;
|
||||||
int c;
|
int c;
|
||||||
char *s;
|
char *s;
|
||||||
|
|
||||||
if (_port == NULL) {
|
if (_port == NULL) {
|
||||||
// default to main serial port
|
// default to main serial port
|
||||||
_port = &Serial;
|
_port = hal.console;
|
||||||
}
|
}
|
||||||
|
|
||||||
// loop performing commands
|
// loop performing commands
|
||||||
for (;; ) {
|
for (;;) {
|
||||||
|
|
||||||
// run the pre-prompt function, if one is defined
|
// run the pre-prompt function, if one is defined
|
||||||
if ((NULL != _ppfunc) && !_ppfunc())
|
if ((NULL != _ppfunc) && !_ppfunc())
|
||||||
|
@ -148,7 +151,7 @@ Menu::_help(void)
|
||||||
|
|
||||||
_port->println_P(PSTR("Commands:"));
|
_port->println_P(PSTR("Commands:"));
|
||||||
for (i = 0; i < _entries; i++) {
|
for (i = 0; i < _entries; i++) {
|
||||||
delay(10);
|
hal.scheduler->delay(10);
|
||||||
_port->printf_P(PSTR(" %S\n"), FPSTR(_commands[i].command));
|
_port->printf_P(PSTR(" %S\n"), FPSTR(_commands[i].command));
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
|
@ -17,6 +17,7 @@
|
||||||
#define __AP_MENU_H__
|
#define __AP_MENU_H__
|
||||||
|
|
||||||
#include <inttypes.h>
|
#include <inttypes.h>
|
||||||
|
#include <AP_HAL.h>
|
||||||
|
|
||||||
#define MENU_COMMANDLINE_MAX 32 ///< maximum input line length
|
#define MENU_COMMANDLINE_MAX 32 ///< maximum input line length
|
||||||
#define MENU_ARGS_MAX 4 ///< maximum number of arguments
|
#define MENU_ARGS_MAX 4 ///< maximum number of arguments
|
||||||
|
@ -56,7 +57,7 @@ public:
|
||||||
///
|
///
|
||||||
typedef int8_t (*func)(uint8_t argc, const struct arg *argv);
|
typedef int8_t (*func)(uint8_t argc, const struct arg *argv);
|
||||||
|
|
||||||
static void set_port(FastSerial *port) {
|
static void set_port(AP_HAL::BetterStream *port) {
|
||||||
_port = port;
|
_port = port;
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -126,7 +127,7 @@ private:
|
||||||
static arg _argv[MENU_ARGS_MAX + 1]; ///< arguments
|
static arg _argv[MENU_ARGS_MAX + 1]; ///< arguments
|
||||||
|
|
||||||
// port to run on
|
// port to run on
|
||||||
static FastSerial *_port;
|
static AP_HAL::BetterStream *_port;
|
||||||
};
|
};
|
||||||
|
|
||||||
/// Macros used to define a menu.
|
/// Macros used to define a menu.
|
||||||
|
|
|
@ -1,27 +1,28 @@
|
||||||
|
|
||||||
#include <FastSerial.h>
|
|
||||||
#include <AP_Common.h>
|
#include <AP_Common.h>
|
||||||
#include <AP_Math.h>
|
#include <AP_Progmem.h>
|
||||||
#include <AP_Menu.h>
|
#include <AP_Menu.h>
|
||||||
|
#include <AP_HAL.h>
|
||||||
|
#include <AP_HAL_AVR.h>
|
||||||
|
|
||||||
FastSerialPort0(Serial);
|
const AP_HAL::HAL& hal = AP_HAL_AVR_APM2;
|
||||||
|
|
||||||
int8_t
|
int8_t
|
||||||
menu_test(uint8_t argc, const Menu::arg *argv)
|
menu_test(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
int i;
|
int i;
|
||||||
|
|
||||||
Serial.printf("This is a test with %d arguments\n", argc);
|
hal.console->printf("This is a test with %d arguments\n", argc);
|
||||||
for (i = 1; i < argc; i++) {
|
for (i = 1; i < argc; i++) {
|
||||||
Serial.printf("%d: int %ld float ", i, argv[i].i);
|
hal.console->printf("%d: int %ld float ", i, argv[i].i);
|
||||||
Serial.println(argv[i].f, 6); // gross
|
hal.console->println(argv[i].f, 6); // gross
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
||||||
int8_t
|
int8_t
|
||||||
menu_auto(uint8_t argc, const Menu::arg *argv)
|
menu_auto(uint8_t argc, const Menu::arg *argv)
|
||||||
{
|
{
|
||||||
Serial.println("auto text");
|
hal.console->println("auto text");
|
||||||
}
|
}
|
||||||
|
|
||||||
const struct Menu::command top_menu_commands[] PROGMEM = {
|
const struct Menu::command top_menu_commands[] PROGMEM = {
|
||||||
|
@ -34,7 +35,7 @@ MENU(top, "menu", top_menu_commands);
|
||||||
void
|
void
|
||||||
setup(void)
|
setup(void)
|
||||||
{
|
{
|
||||||
Serial.begin(38400);
|
hal.console->println_P(PSTR("AP_Menu unit test"));
|
||||||
top.run();
|
top.run();
|
||||||
}
|
}
|
||||||
|
|
||||||
|
@ -43,3 +44,4 @@ loop(void)
|
||||||
{
|
{
|
||||||
}
|
}
|
||||||
|
|
||||||
|
AP_HAL_MAIN();
|
||||||
|
|
Loading…
Reference in New Issue