diff --git a/libraries/SITL/SIM_last_letter.cpp b/libraries/SITL/SIM_last_letter.cpp index e6b0dffa86..92fd978b03 100644 --- a/libraries/SITL/SIM_last_letter.cpp +++ b/libraries/SITL/SIM_last_letter.cpp @@ -30,10 +30,11 @@ extern const AP_HAL::HAL& hal; /* constructor */ -last_letter::last_letter(const char *home_str, const char *frame_str) : - Aircraft(home_str, frame_str), +last_letter::last_letter(const char *home_str, const char *_frame_str) : + Aircraft(home_str, _frame_str), last_timestamp_us(0), - sock(true) + sock(true), + frame_str(_frame_str) { // try to bind to a specific port so that if we restart ArduPilot // last_letter keeps sending us packets. Not strictly necessary but @@ -52,31 +53,39 @@ last_letter::last_letter(const char *home_str, const char *frame_str) : void last_letter::start_last_letter(void) { pid_t child_pid = fork(); - if (child_pid == 0) { - close(0); - open("/dev/null", O_RDONLY); - // in child - for (uint8_t i=3; i<100; i++) { - close(i); - } - - char argHome[50]; - sprintf(argHome,"home:=[%f,%f,%f]",home.lat*1.0e-7,home.lng*1.0e-7,(double)home.alt*1.0e-2); - - int ret = execlp("roslaunch", - "roslaunch", - "last_letter", - "launcher.launch", - "ArduPlane:=true", - "simRate:=500", - "deltaT:=0.002", - argHome, - NULL); - if (ret != 0) { - perror("roslaunch"); - } - exit(1); + if (child_pid != 0) { + return; } + + // in child + close(0); + open("/dev/null", O_RDONLY); + for (uint8_t i=3; i<100; i++) { + close(i); + } + + char argHome[50]; + sprintf(argHome,"home:=[%f,%f,%f]",home.lat*1.0e-7,home.lng*1.0e-7,(double)home.alt*1.0e-2); + + const char *uav_name = strchr(frame_str, ':'); + if (uav_name != NULL) { + uav_name++; + } + + int ret = execlp("roslaunch", + "roslaunch", + "last_letter", + "launcher.launch", + "ArduPlane:=true", + "simRate:=500", + "deltaT:=0.002", + argHome, + uav_name, + NULL); + if (ret != 0) { + perror("roslaunch"); + } + exit(1); } /* diff --git a/libraries/SITL/SIM_last_letter.h b/libraries/SITL/SIM_last_letter.h index da3c7efd8c..40bfa1b518 100644 --- a/libraries/SITL/SIM_last_letter.h +++ b/libraries/SITL/SIM_last_letter.h @@ -70,6 +70,8 @@ private: uint64_t last_timestamp_us; SocketAPM sock; + + const char *frame_str; };