diff --git a/src/flight_tests/ft_2024_03_16.py b/src/flight_tests/ft_2024_03_16.py
index 7042a3e..f60d738 100644
--- a/src/flight_tests/ft_2024_03_16.py
+++ b/src/flight_tests/ft_2024_03_16.py
@@ -22,7 +22,8 @@
 time.sleep(2)
 
 nav.takeoff(10)
-start_coords = drone.location.global_relative_frame
+drone.groundspeed = 2  # m/s
+# start_coords = drone.location.global_relative_frame
 time.sleep(2)
 
 nav.set_altitude_position_relative(-10, 0, 10)
@@ -31,13 +32,21 @@
 nav.set_position_relative(0, -10)
 time.sleep(1)
 
-lander.generateRoute(2)
+nav.send_status_message("Executing landing pad search")
+lander.generateRoute(4)
 
 for route in lander.route:
-    lander.goNext(nav, route, 5)
+    lander.goNext(nav, route, 10)
     time.sleep(3)
 
-nav.set_position(start_coords.lat, start_coords.lon)
-time.sleep(1)
+# nav.set_position(start_coords.lat, start_coords.lon)
+# time.sleep(1)
+# nav.land()
+
+nav.return_to_launch()
+
+drone.close()
+
+nav.send_status_message("Flight test script execution terminated")
+
 
-nav.land()
diff --git a/src/modules/autopilot/navigator.py b/src/modules/autopilot/navigator.py
index f0bc088..9ca2f69 100644
--- a/src/modules/autopilot/navigator.py
+++ b/src/modules/autopilot/navigator.py
@@ -13,6 +13,7 @@ class Navigator:
     """
 
     vehicle: dronekit.Vehicle = None
+    POSITION_TOLERANCE = 1.0  # m
 
     def __init__(self, vehicle, messenger_port):
         self.vehicle = vehicle
@@ -70,7 +71,7 @@ def set_position(self, lat, lon):
             remaining_distance = self.__get_distance_metres(
                 self.vehicle.location.global_relative_frame, target_location)
             self.__message(f"Distance to target: {remaining_distance} m")
-            if remaining_distance <= 0.1:
+            if remaining_distance <= self.POSITION_TOLERANCE:
                 self.__message("Reached target")
                 break
             time.sleep(2)
@@ -98,7 +99,7 @@ def set_position_relative(self, d_north, d_east):
             remaining_distance = self.__get_distance_metres(
                 self.vehicle.location.global_relative_frame, target_location)
             self.__message(f"Distance to target: {remaining_distance} m")
-            if remaining_distance <= target_distance * 0.1:
+            if remaining_distance <= self.POSITION_TOLERANCE:
                 self.__message("Reached target")
                 break
             time.sleep(2)
@@ -187,9 +188,19 @@ def set_altitude_position_relative(self, d_north, d_east, alt):
         current_location = self.vehicle.location.global_relative_frame
         target_location = self.__get_location_metres(current_location, d_north, d_east)
         target_location.alt += alt
+        target_distance = self.__get_distance_metres(current_location, target_location)
 
         self.vehicle.simple_goto(target_location)
 
+        while self.vehicle.mode.name == "GUIDED":
+            remaining_distance = self.__get_distance_metres(
+                self.vehicle.location.global_relative_frame, target_location)
+            self.__message(f"Distance to target: {remaining_distance} m")
+            if remaining_distance <= self.POSITION_TOLERANCE:
+                self.__message("Reached target")
+                break
+            time.sleep(2)
+
     def land(self):
         """
         Lands the vehicle.