diff --git a/ROMFS/px4fmu_common/init.d-posix/rcS b/ROMFS/px4fmu_common/init.d-posix/rcS
index 8ce56eb640..efa5dcd55c 100644
--- a/ROMFS/px4fmu_common/init.d-posix/rcS
+++ b/ROMFS/px4fmu_common/init.d-posix/rcS
@@ -187,15 +187,15 @@ param set-default UXRCE_DDS_AG_IP 2130706433  # 127.0.0.1
 
 # Adapt timeout parameters if simulation runs faster or slower than realtime.
 if [ -n "$PX4_SIM_SPEED_FACTOR" ]; then
-	COM_DL_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 10" | bc)
+	COM_DL_LOSS_T_LONGER=$((PX4_SIM_SPEED_FACTOR * 10))
 	echo "COM_DL_LOSS_T set to $COM_DL_LOSS_T_LONGER"
 	param set COM_DL_LOSS_T $COM_DL_LOSS_T_LONGER
 
-	COM_RC_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 1.0" | bc)
+	COM_RC_LOSS_T_LONGER=$((PX4_SIM_SPEED_FACTOR))
 	echo "COM_RC_LOSS_T set to $COM_RC_LOSS_T_LONGER"
 	param set COM_RC_LOSS_T $COM_RC_LOSS_T_LONGER
 
-	COM_OF_LOSS_T_LONGER=$(echo "$PX4_SIM_SPEED_FACTOR * 1.0" | bc)
+	COM_OF_LOSS_T_LONGER=$((PX4_SIM_SPEED_FACTOR))
 	echo "COM_OF_LOSS_T set to $COM_OF_LOSS_T_LONGER"
 	param set COM_OF_LOSS_T $COM_OF_LOSS_T_LONGER
 fi
@@ -231,7 +231,7 @@ env | while IFS='=' read -r line; do
   value=${line#*=}
   name=${line%%=*}
   case $name in
-    "PX4_PARAM_"*) param set "${name#PX4_PARAM_}" "$value" ;;
+    "PX4_PARAM_"*) param set "${name#PX4_PARAM_}" "$value" < /dev/null ;;
   esac
 done
 
diff --git a/test/mavsdk_tests/integration_test_runner/process_helper.py b/test/mavsdk_tests/integration_test_runner/process_helper.py
index b399a4756d..3fc3263102 100644
--- a/test/mavsdk_tests/integration_test_runner/process_helper.py
+++ b/test/mavsdk_tests/integration_test_runner/process_helper.py
@@ -69,6 +69,9 @@ class Runner:
     def has_started_ok(self) -> bool:
         return True
 
+    def ready_to_start(self) -> bool:
+        return True
+
     def process_output(self) -> None:
         assert self.process.stdout is not None
         while True:
@@ -166,6 +169,8 @@ class Px4Runner(Runner):
                 "etc/init.d-posix/rcS",
                 "-t",
                 os.path.join(workspace_dir, "test_data"),
+                "-w",
+                self.cwd,
                 "-d"
             ]
         self.env["PX4_SIM_MODEL"] = "gazebo-classic_" + self.model
@@ -377,7 +382,11 @@ class TestRunnerMavsdk(Runner):
                      "--speed-factor", str(speed_factor),
                      case]
 
+
 class TestRunnerRos(Runner):
+    _PX4_ROS2_READY_TIMEOUT_S = 30
+    _PX4_ROS2_READY_MARKER = "successfully created rt/fmu/out/vehicle_status"
+
     def __init__(self,
                  workspace_dir: str,
                  log_dir: str,
@@ -395,6 +404,44 @@ class TestRunnerRos(Runner):
                          "integration_tests"),
                      "--gtest_filter="+case, "--gtest_color=yes"]
 
+    def ready_to_start(self) -> bool:
+        px4_log_filename = os.path.join(self.log_dir, "log-px4.log")
+        deadline = time.time() + self._PX4_ROS2_READY_TIMEOUT_S
+        offset = 0
+
+        if self.verbose:
+            print("Waiting for PX4 ROS 2 vehicle_status publisher")
+
+        while time.time() < deadline:
+            try:
+                with open(px4_log_filename, 'r') as px4_log:
+                    px4_log.seek(offset)
+
+                    for line in px4_log:
+                        if self._PX4_ROS2_READY_MARKER in line:
+                            if self.verbose:
+                                print("PX4 ROS 2 vehicle_status publisher "
+                                      "is ready")
+                            return True
+
+                        if "PX4 Exiting" in line:
+                            print("PX4 exited before ROS 2 vehicle_status "
+                                  "publisher was ready")
+                            return False
+
+                    offset = px4_log.tell()
+
+            except FileNotFoundError:
+                pass
+
+            time.sleep(0.1)
+
+        message = ("Timed out waiting {}s for PX4 ROS 2 vehicle_status "
+                   "publisher in {}").format(
+                       self._PX4_ROS2_READY_TIMEOUT_S, px4_log_filename)
+        print(message)
+        return False
+
     def get_output_line(self) -> Optional[str]:
         line = super().get_output_line()
         if line is not None:
diff --git a/test/mavsdk_tests/integration_test_runner/test_runner.py b/test/mavsdk_tests/integration_test_runner/test_runner.py
index d48ec73402..ce3e396254 100644
--- a/test/mavsdk_tests/integration_test_runner/test_runner.py
+++ b/test/mavsdk_tests/integration_test_runner/test_runner.py
@@ -391,6 +391,9 @@ class Tester:
 
     def try_to_run_several_times(self, runner: ph.Runner) -> bool:
         for _ in range(3):
+            if not runner.ready_to_start():
+                return False
+
             runner.start()
 
             if runner.has_started_ok():