diff --git a/launch/single_vehicle_spawn.launch b/launch/single_vehicle_spawn.launch
index ae2fbf184c..5845f346ef 100644
--- a/launch/single_vehicle_spawn.launch
+++ b/launch/single_vehicle_spawn.launch
@@ -25,7 +25,7 @@
     <!-- PX4 SITL -->
     <arg unless="$(arg interactive)" name="px4_command_arg1" value=""/>
     <arg     if="$(arg interactive)" name="px4_command_arg1" value="-d"/>
-    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -i $(arg ID) $(arg px4_command_arg1)">
+    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -i $(arg ID) -w sitl_$(arg vehicle)_$(arg ID) $(arg px4_command_arg1)">
     </node>
     <!-- spawn vehicle -->
     <node name="$(arg vehicle)_$(arg ID)_spawn" output="screen" pkg="gazebo_ros" type="spawn_model" args="-urdf -param rotors_description -model $(arg vehicle)_$(arg ID) -package_to_model -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
diff --git a/launch/single_vehicle_spawn_sdf.launch b/launch/single_vehicle_spawn_sdf.launch
index 3c4253ea93..b55eef2c1f 100644
--- a/launch/single_vehicle_spawn_sdf.launch
+++ b/launch/single_vehicle_spawn_sdf.launch
@@ -25,7 +25,7 @@
     <!-- PX4 SITL -->
     <arg unless="$(arg interactive)" name="px4_command_arg1" value=""/>
     <arg     if="$(arg interactive)" name="px4_command_arg1" value="-d"/>
-    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -i $(arg ID) $(arg px4_command_arg1)">
+    <node name="sitl_$(arg ID)" pkg="px4" type="px4" output="screen" args="$(find px4)/ROMFS/px4fmu_common -s etc/init.d-posix/rcS -i $(arg ID) -w sitl_$(arg vehicle)_$(arg ID) $(arg px4_command_arg1)">
     </node>
     <!-- spawn vehicle -->
     <node name="$(arg vehicle)_$(arg ID)_spawn" output="screen" pkg="gazebo_ros" type="spawn_model" args="-sdf -param model_description -model $(arg vehicle)_$(arg ID) -x $(arg x) -y $(arg y) -z $(arg z) -R $(arg R) -P $(arg P) -Y $(arg Y)"/>
diff --git a/platforms/posix/src/main.cpp b/platforms/posix/src/main.cpp
index 63f3532c06..21763ae2af 100644
--- a/platforms/posix/src/main.cpp
+++ b/platforms/posix/src/main.cpp
@@ -109,6 +109,7 @@ static bool dir_exists(const std::string &path);
 static bool file_exists(const std::string &name);
 static std::string file_basename(std::string const &pathname);
 static std::string pwd();
+static int change_directory(const std::string &directory);
 
 
 #ifdef __PX4_SITL_MAIN_OVERRIDE
@@ -181,13 +182,14 @@ int main(int argc, char **argv)
 		std::string data_path;
 		std::string commands_file = "etc/init.d/rcS";
 		std::string test_data_path;
+		std::string working_directory;
 		int instance = 0;
 
 		int myoptind = 1;
 		int ch;
 		const char *myoptarg = nullptr;
 
-		while ((ch = px4_getopt(argc, argv, "hdt:s:i:", &myoptind, &myoptarg)) != EOF) {
+		while ((ch = px4_getopt(argc, argv, "hdt:s:i:w:", &myoptind, &myoptarg)) != EOF) {
 			switch (ch) {
 			case 'h':
 				print_usage();
@@ -209,6 +211,10 @@ int main(int argc, char **argv)
 				instance = strtoul(myoptarg, nullptr, 10);
 				break;
 
+			case 'w':
+				working_directory = myoptarg;
+				break;
+
 			default:
 				PX4_ERR("unrecognized flag");
 				print_usage();
@@ -218,6 +224,15 @@ int main(int argc, char **argv)
 
 		PX4_DEBUG("instance: %i", instance);
 
+		// change the CWD befre setting up links and other directories
+		if (!working_directory.empty()) {
+			int ret = change_directory(working_directory);
+
+			if (ret != PX4_OK) {
+				return ret;
+			}
+		}
+
 		if (myoptind < argc) {
 			std::string optional_arg = argv[myoptind];
 
@@ -232,7 +247,6 @@ int main(int argc, char **argv)
 			return -1;
 		}
 
-
 		int ret = create_symlinks_if_needed(data_path);
 
 		if (ret != PX4_OK) {
@@ -383,7 +397,6 @@ int create_dirs()
 	return PX4_OK;
 }
 
-
 void register_sig_handler()
 {
 	// SIGINT
@@ -544,14 +557,15 @@ void print_usage()
 {
 	printf("Usage for Server/daemon process: \n");
 	printf("\n");
-	printf("    px4 [-h|-d] [-s <startup_file>] [-t <test_data_directory>] [<rootfs_directory>] [-i <instance>]\n");
+	printf("    px4 [-h|-d] [-s <startup_file>] [-t <test_data_directory>] [<rootfs_directory>] [-i <instance>] [-w <working_directory>]\n");
 	printf("\n");
-	printf("    -s <startup_file>  shell script to be used as startup (default=etc/init.d/rcS)\n");
-	printf("    <rootfs_directory> directory where startup files and mixers are located,\n");
-	printf("                       (if not given, CWD is used)\n");
-	printf("    -i <instance>      px4 instance id to run multiple instances [0...N], default=0\n");
-	printf("    -h                 help/usage information\n");
-	printf("    -d                 daemon mode, don't start pxh shell\n");
+	printf("    -s <startup_file>      shell script to be used as startup (default=etc/init.d/rcS)\n");
+	printf("    <rootfs_directory>     directory where startup files and mixers are located,\n");
+	printf("                           (if not given, CWD is used)\n");
+	printf("    -i <instance>          px4 instance id to run multiple instances [0...N], default=0\n");
+	printf("    -w <working_directory> directory to change to\n");
+	printf("    -h                     help/usage information\n");
+	printf("    -d                     daemon mode, don't start pxh shell\n");
 	printf("\n");
 	printf("Usage for client: \n");
 	printf("\n");
@@ -626,3 +640,26 @@ std::string pwd()
 	char temp[PATH_MAX];
 	return (getcwd(temp, PATH_MAX) ? std::string(temp) : std::string(""));
 }
+
+int change_directory(const std::string &directory)
+{
+	// create directory
+	if (!dir_exists(directory)) {
+		int ret = mkdir(directory.c_str(), S_IRWXU | S_IRWXG | S_IRWXO);
+
+		if (ret == -1) {
+			PX4_ERR("Error creating directory: %s (%s)", directory.c_str(), strerror(errno));
+			return -1;
+		}
+	}
+
+	// change directory
+	int ret = chdir(directory.c_str());
+
+	if (ret == -1) {
+		PX4_ERR("Error changing current path to: %s (%s)", directory.c_str(), strerror(errno));
+		return -1;
+	}
+
+	return PX4_OK;
+}