diff --git a/libraries/AP_HAL_SITL/SITL_State.h b/libraries/AP_HAL_SITL/SITL_State.h index 70080b23a3..5da8ed5896 100644 --- a/libraries/AP_HAL_SITL/SITL_State.h +++ b/libraries/AP_HAL_SITL/SITL_State.h @@ -118,6 +118,11 @@ public: Location &loc, float &yaw_degrees); + /* lookup a location in locations.txt */ + static bool lookup_location(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 989d91882f..7249089b90 100644 --- a/libraries/AP_HAL_SITL/SITL_cmdline.cpp +++ b/libraries/AP_HAL_SITL/SITL_cmdline.cpp @@ -35,6 +35,7 @@ #include #include #include +#include #include #include @@ -74,7 +75,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 start location (lat,lng,alt,yaw)\n" + "\t--home|-O HOME set start location (lat,lng,alt,yaw) or location name\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" @@ -435,7 +436,12 @@ void SITL_State::_parse_command_line(int argc, char * const argv[]) if (home_str != nullptr) { Location home; float home_yaw; - if (!parse_home(home_str, home, home_yaw)) { + if (strchr(home_str,',') == nullptr) { + if (!lookup_location(home_str, home, home_yaw)) { + ::printf("Failed to find location (%s). Should be in locations.txt or LAT,LON,ALT,HDG e.g. 37.4003371,-122.0800351,0,353\n", home_str); + exit(1); + } + } else 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); } @@ -545,4 +551,38 @@ bool SITL_State::parse_home(const char *home_str, Location &loc, float &yaw_degr return true; } +/* + lookup a location in locations.txt in ROMFS + */ +bool SITL_State::lookup_location(const char *home_str, Location &loc, float &yaw_degrees) +{ + const char *locations = "@ROMFS/locations.txt"; + FileData *fd = AP::FS().load_file(locations); + if (fd == nullptr) { + ::printf("Missing %s\n", locations); + return false; + } + char *str = strndup((const char *)fd->data, fd->length); + if (!str) { + delete fd; + return false; + } + size_t len = strlen(home_str); + char *saveptr = nullptr; + for (char *s = strtok_r(str, "\r\n", &saveptr); + s; + s=strtok_r(nullptr, "\r\n", &saveptr)) { + if (strncasecmp(s, home_str, len) == 0 && s[len]=='=') { + bool ok = parse_home(&s[len+1], loc, yaw_degrees); + free(str); + delete fd; + return ok; + } + } + free(str); + delete fd; + ::printf("Failed to find location '%s'\n", home_str); + return false; +} + #endif