From 94d2ce13d2d3c8b0ba36079079de1d2b17b1dcbc Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Thu, 15 Aug 2019 16:52:50 +1000 Subject: [PATCH] AP_HAL_SITL: allow starting location to come from parameters --- libraries/AP_HAL_SITL/SITL_State.cpp | 2 +- libraries/AP_HAL_SITL/SITL_State.h | 7 ++- libraries/AP_HAL_SITL/SITL_cmdline.cpp | 72 ++++++++++++++++++++++++-- 3 files changed, 75 insertions(+), 6 deletions(-) diff --git a/libraries/AP_HAL_SITL/SITL_State.cpp b/libraries/AP_HAL_SITL/SITL_State.cpp index 5f7d7171a7..ebc46bf691 100644 --- a/libraries/AP_HAL_SITL/SITL_State.cpp +++ b/libraries/AP_HAL_SITL/SITL_State.cpp @@ -331,7 +331,7 @@ void SITL_State::_fdm_input_local(void) _simulator_servos(input); // update the model - sitl_model->update(input); + sitl_model->update_model(input); // get FDM output from the model if (_sitl) { diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index bd76094a01..f3f170fead 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -81,7 +81,12 @@ public: "tcp:5", "tcp:6", }; - + + /* parse a home location string */ + static bool parse_home(const char *home_str, + Location &loc, + float &yaw_degrees); + private: void _parse_command_line(int argc, char * const argv[]); void _set_param_default(const char *parm); diff --git a/libraries/AP_HAL_SITL/SITL_cmdline.cpp b/libraries/AP_HAL_SITL/SITL_cmdline.cpp index 5926efeb2c..12b245200a 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -68,7 +68,7 @@ void SITL_State::_usage(void) "\t--instance|-I N set instance of SITL (adds 10*instance to all port numbers)\n" // "\t--param|-P NAME=VALUE set some param\n" CURRENTLY BROKEN! "\t--synthetic-clock|-S set synthetic clock mode\n" - "\t--home|-O HOME set home location (lat,lng,alt,yaw)\n" + "\t--home|-O HOME set start location (lat,lng,alt,yaw)\n" "\t--model|-M MODEL set simulation model\n" "\t--config string set additional simulation config string\n" "\t--fg|-F ADDRESS set Flight Gear view address, defaults to 127.0.0.1\n" @@ -163,7 +163,7 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) _instance = 0; _synthetic_clock_mode = false; // default to CMAC - const char *home_str = "-35.363261,149.165230,584,353"; + const char *home_str = nullptr; const char *model_str = nullptr; _use_fg_view = true; char *autotest_dir = nullptr; @@ -371,9 +371,17 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) for (uint8_t i=0; i < ARRAY_SIZE(model_constructors); i++) { if (strncasecmp(model_constructors[i].name, model_str, strlen(model_constructors[i].name)) == 0) { - printf("Creating model %s at speed %.1f\n", model_str, speedup); + // printf("Creating model %f,%f,%f,%f at speed %.1f\n", opos.lat, opos.lng, opos.alt, opos.hdg, speedup); sitl_model = model_constructors[i].constructor(model_str); - sitl_model->set_start_location(home_str); + if (home_str != nullptr) { + Location home; + float home_yaw; + if (!parse_home(home_str, home, home_yaw)) { + ::printf("Failed to parse home string (%s). Should be LAT,LON,ALT,HDG e.g. 37.4003371,-122.0800351,0,353\n", home_str); + exit(1); + } + sitl_model->set_start_location(home, home_yaw); + } sitl_model->set_interface_ports(simulator_address, simulator_port_in, simulator_port_out); sitl_model->set_speedup(speedup); sitl_model->set_instance(_instance); @@ -417,4 +425,60 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) _sitl_setup(home_str); } +/* + parse a home string into a location and yaw + */ +bool SITL_State::parse_home(const char *home_str, Location &loc, float &yaw_degrees) +{ + char *saveptr = nullptr; + char *s = strdup(home_str); + if (!s) { + free(s); + ::printf("No home string supplied\n"); + return false; + } + char *lat_s = strtok_r(s, ",", &saveptr); + if (!lat_s) { + free(s); + ::printf("Failed to parse latitude\n"); + return false; + } + char *lon_s = strtok_r(nullptr, ",", &saveptr); + if (!lon_s) { + free(s); + ::printf("Failed to parse longitude\n"); + return false; + } + char *alt_s = strtok_r(nullptr, ",", &saveptr); + if (!alt_s) { + free(s); + ::printf("Failed to parse altitude\n"); + return false; + } + char *yaw_s = strtok_r(nullptr, ",", &saveptr); + if (!yaw_s) { + free(s); + ::printf("Failed to parse yaw\n"); + return false; + } + + loc = {}; + loc.lat = static_cast(strtod(lat_s, nullptr) * 1.0e7); + loc.lng = static_cast(strtod(lon_s, nullptr) * 1.0e7); + loc.alt = static_cast(strtod(alt_s, nullptr) * 1.0e2); + + if (loc.lat == 0 && loc.lng == 0) { + // default to CMAC instead of middle of the ocean. This makes + // SITL in MissionPlanner a bit more useful + loc.lat = -35.363261*1e7; + loc.lng = 149.165230*1e7; + loc.alt = 584*100; + } + + yaw_degrees = strtof(yaw_s, nullptr); + free(s); + + return true; +} + #endif