SITL: make home location parsing verbose

Given we don't fail to start SITL if the home location fails to parse,
we should at least make the user very aware that their home location
string is bad.

This commit makes failures verbose, and outputs the parsed location
values rather than regurgitating the location string the user supplied;
it was very misleading in that the location could simply fail to parse
and thus be unused.
This commit is contained in:
Peter Barker 2018-02-20 12:33:14 +11:00 committed by Randy Mackay
parent d1a96ca4ff
commit 6a862c6e24
2 changed files with 15 additions and 2 deletions

View File

@ -343,13 +343,13 @@ 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);
sitl_model = model_constructors[i].constructor(home_str, model_str);
sitl_model->set_interface_ports(_simulator_address, _simulator_port_in, _simulator_port_out);
sitl_model->set_speedup(speedup);
sitl_model->set_instance(_instance);
sitl_model->set_autotest_dir(autotest_dir);
_synthetic_clock_mode = true;
printf("Started model %s at %s at speed %.1f\n", model_str, home_str, speedup);
break;
}
}

View File

@ -62,7 +62,15 @@ Aircraft::Aircraft(const char *home_str, const char *frame_str) :
{
// make the SIM_* variables available to simulator backends
sitl = (SITL *)AP_Param::find_object("SIM_");
parse_home(home_str, home, 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);
}
::printf("Home: %f %f alt=%fm hdg=%f\n",
home.lat*1e-7,
home.lng*1e-7,
home.alt*0.01,
home_yaw);
location = home;
ground_level = home.alt * 0.01f;
@ -90,26 +98,31 @@ bool Aircraft::parse_home(const char *home_str, Location &loc, float &yaw_degree
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;
}