From 5158f91263b2aeb4719945b94287c470177c6778 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Tue, 5 May 2015 07:08:42 +1000 Subject: [PATCH] HAL_SITL: converted command line code to getopt_cpp --- libraries/AP_HAL_SITL/SITL_State.cpp | 174 +----------------------- libraries/AP_HAL_SITL/SITL_State.h | 2 +- libraries/AP_HAL_SITL/SITL_cmdline.cpp | 181 +++++++++++++++++++++++++ 3 files changed, 186 insertions(+), 171 deletions(-) create mode 100644 libraries/AP_HAL_SITL/SITL_cmdline.cpp diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index b22719c54f..05bf30c9cc 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -12,7 +12,6 @@ #include #include -#include #include #include #include @@ -20,180 +19,14 @@ #include -#include -#include -#include - -#ifdef __CYGWIN__ -#include -#include -#include -#include -#include - -void print_trace() { - char pid_buf[30]; - sprintf(pid_buf, "%d", getpid()); - char name_buf[512]; - name_buf[readlink("/proc/self/exe", name_buf, 511)]=0; - int child_pid = fork(); - if (!child_pid) { - dup2(2,1); // redirect output to stderr - fprintf(stdout,"stack trace for %s pid=%s\n",name_buf,pid_buf); - execlp("gdb", "gdb", "--batch", "-n", "-ex", "thread", "-ex", "bt", name_buf, pid_buf, NULL); - abort(); /* If gdb failed to start */ - } else { - waitpid(child_pid,NULL,0); - } -} -#endif - extern const AP_HAL::HAL& hal; using namespace HALSITL; -// catch floating point exceptions -static void _sig_fpe(int signum) +void SITL_State::_set_param_default(const char *parm) { - fprintf(stderr, "ERROR: Floating point exception - aborting\n"); - abort(); -} - -void SITL_State::_usage(void) -{ - fprintf(stdout, "Options:\n"); - fprintf(stdout, "\t-w wipe eeprom and dataflash\n"); - fprintf(stdout, "\t-r RATE set SITL framerate\n"); - fprintf(stdout, "\t-H HEIGHT initial barometric height\n"); - fprintf(stdout, "\t-C use console instead of TCP ports\n"); - fprintf(stdout, "\t-I set instance of SITL (adds 10*instance to all port numbers)\n"); - fprintf(stdout, "\t-s SPEEDUP simulation speedup\n"); - fprintf(stdout, "\t-O ORIGIN set home location (lat,lng,alt,yaw)\n"); - fprintf(stdout, "\t-M MODEL set simulation model\n"); - fprintf(stdout, "\t-F FDMADDR set FDM UDP address (IPv4)\n"); -} - -static const struct { - const char *name; - Aircraft *(*constructor)(const char *home_str, const char *frame_str); -} model_constructors[] = { - { "+", MultiCopter::create }, - { "x", MultiCopter::create }, - { "hexa", MultiCopter::create }, - { "hexax", MultiCopter::create }, - { "octa", MultiCopter::create }, - { "octa-quad", MultiCopter::create }, - { "heli", Helicopter::create }, - { "rover", Rover::create }, - { "rover-skid",Rover::create } -}; - -void SITL_State::_parse_command_line(int argc, char * const argv[]) -{ - int opt; - const char *home_str = NULL; - const char *model_str = NULL; - float speedup = 1.0f; - - signal(SIGFPE, _sig_fpe); - // No-op SIGPIPE handler - signal(SIGPIPE, SIG_IGN); - - setvbuf(stdout, (char *)0, _IONBF, 0); - setvbuf(stderr, (char *)0, _IONBF, 0); - - _synthetic_clock_mode = false; - _base_port = 5760; - _rcout_port = 5502; - _simin_port = 5501; - _fdm_address = "127.0.0.1"; - - while ((opt = getopt(argc, argv, "s:whr:H:CI:P:SO:M:S:F:")) != -1) { - switch (opt) { - case 'w': - AP_Param::erase_all(); - unlink("dataflash.bin"); - break; - case 'r': - _framerate = (unsigned)atoi(optarg); - break; - case 'H': - _initial_height = atof(optarg); - break; - case 'C': - HALSITL::SITLUARTDriver::_console = true; - break; - case 'I': { - uint8_t instance = atoi(optarg); - _base_port += instance * 10; - _rcout_port += instance * 10; - _simin_port += instance * 10; - } - break; - case 'P': - _set_param_default(optarg); - break; - case 'S': - _synthetic_clock_mode = true; - break; - case 'O': - home_str = optarg; - break; - case 'M': - model_str = optarg; - break; - case 's': - speedup = atof(optarg); - break; - case 'F': - _fdm_address = optarg; - break; - default: - _usage(); - exit(1); - } - } - - if (model_str && home_str) { - for (uint8_t i=0; iset_speedup(speedup); - _synthetic_clock_mode = true; - printf("Started model %s at %s at speed %.1f\n", model_str, home_str, speedup); - break; - } - } - } - - fprintf(stdout, "Starting sketch '%s'\n", SKETCH); - - if (strcmp(SKETCH, "ArduCopter") == 0) { - _vehicle = ArduCopter; - if (_framerate == 0) { - _framerate = 200; - } - } else if (strcmp(SKETCH, "APMrover2") == 0) { - _vehicle = APMrover2; - if (_framerate == 0) { - _framerate = 50; - } - // set right default throttle for rover (allowing for reverse) - pwm_input[2] = 1500; - } else { - _vehicle = ArduPlane; - if (_framerate == 0) { - _framerate = 50; - } - } - - _sitl_setup(); -} - - -void SITL_State::_set_param_default(char *parm) -{ - char *p = strchr(parm, '='); + char *pdup = strdup(parm); + char *p = strchr(pdup, '='); if (p == NULL) { printf("Please specify parameter as NAME=VALUE"); exit(1); @@ -219,6 +52,7 @@ void SITL_State::_set_param_default(char *parm) exit(1); } printf("Set parameter %s to %f\n", parm, value); + free(pdup); } diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 72420fa0d5..fb14ff33af 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -55,7 +55,7 @@ public: private: void _parse_command_line(int argc, char * const argv[]); - void _set_param_default(char *parm); + void _set_param_default(const char *parm); void _usage(void); void _sitl_setup(void); void _setup_fdm(void); diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp new file mode 100644 index 0000000000..7ef74ce534 --- /dev/null +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -0,0 +1,181 @@ +/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*- + +#include + +#if CONFIG_HAL_BOARD == HAL_BOARD_SITL + +#include +#include "AP_HAL_SITL_Namespace.h" +#include "HAL_SITL_Class.h" +#include "UARTDriver.h" +#include +#include +#include +#include + +#include +#include +#include + +extern const AP_HAL::HAL& hal; + +using namespace HALSITL; + +// catch floating point exceptions +static void _sig_fpe(int signum) +{ + fprintf(stderr, "ERROR: Floating point exception - aborting\n"); + abort(); +} + +void SITL_State::_usage(void) +{ + fprintf(stdout, "Options:\n"); + fprintf(stdout, "\t-w wipe eeprom and dataflash\n"); + fprintf(stdout, "\t-r RATE set SITL framerate\n"); + fprintf(stdout, "\t-H HEIGHT initial barometric height\n"); + fprintf(stdout, "\t-C use console instead of TCP ports\n"); + fprintf(stdout, "\t-I set instance of SITL (adds 10*instance to all port numbers)\n"); + fprintf(stdout, "\t-s SPEEDUP simulation speedup\n"); + fprintf(stdout, "\t-O ORIGIN set home location (lat,lng,alt,yaw)\n"); + fprintf(stdout, "\t-M MODEL set simulation model\n"); + fprintf(stdout, "\t-F FDMADDR set FDM UDP address (IPv4)\n"); +} + +static const struct { + const char *name; + Aircraft *(*constructor)(const char *home_str, const char *frame_str); +} model_constructors[] = { + { "+", MultiCopter::create }, + { "x", MultiCopter::create }, + { "hexa", MultiCopter::create }, + { "hexax", MultiCopter::create }, + { "octa", MultiCopter::create }, + { "octa-quad", MultiCopter::create }, + { "heli", Helicopter::create }, + { "rover", Rover::create }, + { "rover-skid",Rover::create } +}; + +void SITL_State::_parse_command_line(int argc, char * const argv[]) +{ + int opt; + const char *home_str = NULL; + const char *model_str = NULL; + float speedup = 1.0f; + + signal(SIGFPE, _sig_fpe); + // No-op SIGPIPE handler + signal(SIGPIPE, SIG_IGN); + + setvbuf(stdout, (char *)0, _IONBF, 0); + setvbuf(stderr, (char *)0, _IONBF, 0); + + _synthetic_clock_mode = false; + _base_port = 5760; + _rcout_port = 5502; + _simin_port = 5501; + _fdm_address = "127.0.0.1"; + + const struct GetOptLong::option options[] = { + {"help", false, 0, 'h'}, + {"wipe", false, 0, 'w'}, + {"speedup", true, 0, 's'}, + {"rate", true, 0, 'r'}, + {"height", true, 0, 'H'}, + {"console", false, 0, 'C'}, + {"instance", true, 0, 'I'}, + {"param", true, 0, 'P'}, + {"synthetic-clock", false, 0, 'S'}, + {"home", true, 0, 'O'}, + {"model", true, 0, 'M'}, + {"frame", true, 0, 'F'}, + {0, false, 0, 0} + }; + + GetOptLong gopt(argc, argv, "hws:r:H:CI:P:SO:M:F:", + options); + + while ((opt = gopt.getoption()) != -1) { + switch (opt) { + case 'w': + AP_Param::erase_all(); + unlink("dataflash.bin"); + break; + case 'r': + _framerate = (unsigned)atoi(gopt.optarg); + break; + case 'H': + _initial_height = atof(gopt.optarg); + break; + case 'C': + HALSITL::SITLUARTDriver::_console = true; + break; + case 'I': { + uint8_t instance = atoi(gopt.optarg); + _base_port += instance * 10; + _rcout_port += instance * 10; + _simin_port += instance * 10; + } + break; + case 'P': + _set_param_default(gopt.optarg); + break; + case 'S': + _synthetic_clock_mode = true; + break; + case 'O': + home_str = gopt.optarg; + break; + case 'M': + model_str = gopt.optarg; + break; + case 's': + speedup = atof(gopt.optarg); + break; + case 'F': + _fdm_address = gopt.optarg; + break; + default: + _usage(); + exit(1); + } + } + + if (model_str && home_str) { + for (uint8_t i=0; iset_speedup(speedup); + _synthetic_clock_mode = true; + printf("Started model %s at %s at speed %.1f\n", model_str, home_str, speedup); + break; + } + } + } + + fprintf(stdout, "Starting sketch '%s'\n", SKETCH); + + if (strcmp(SKETCH, "ArduCopter") == 0) { + _vehicle = ArduCopter; + if (_framerate == 0) { + _framerate = 200; + } + } else if (strcmp(SKETCH, "APMrover2") == 0) { + _vehicle = APMrover2; + if (_framerate == 0) { + _framerate = 50; + } + // set right default throttle for rover (allowing for reverse) + pwm_input[2] = 1500; + } else { + _vehicle = ArduPlane; + if (_framerate == 0) { + _framerate = 50; + } + } + + _sitl_setup(); +} + +#endif