diff --git a/.gitmodules b/.gitmodules
index 99484d6b..9d6ed5c4 100644
--- a/.gitmodules
+++ b/.gitmodules
@@ -4,7 +4,7 @@
 [submodule "controls/sae_2025_ws/src/px4_msgs"]
 	path = controls/sae_2025_ws/src/px4_msgs
 	url = https://github.com/PX4/px4_msgs.git
-	branch = release/1.15
+	branch = release/1.16
 [submodule "controls/sae_2025_ws/src/ros_gz"]
 	path = controls/sae_2025_ws/src/ros_gz
 	url = https://github.com/gazebosim/ros_gz.git
diff --git a/controls/sae_2025_ws/src/px4_msgs b/controls/sae_2025_ws/src/px4_msgs
index 18cbcced..392e831c 160000
--- a/controls/sae_2025_ws/src/px4_msgs
+++ b/controls/sae_2025_ws/src/px4_msgs
@@ -1 +1 @@
-Subproject commit 18cbccedb6c304b180bca8e81a4a601758fc9de9
+Subproject commit 392e831c1f659429ca83902e66820d7094591410
diff --git a/controls/sae_2025_ws/src/ros_gz b/controls/sae_2025_ws/src/ros_gz
index 7075aa51..6be1a56e 160000
--- a/controls/sae_2025_ws/src/ros_gz
+++ b/controls/sae_2025_ws/src/ros_gz
@@ -1 +1 @@
-Subproject commit 7075aa51934c0cf0f9f04062f8643c6f8b701fea
+Subproject commit 6be1a56e797d4a9e94c9aff280a943df3dfa6d48
diff --git a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml
index 4841f393..44ae2fff 100644
--- a/controls/sae_2025_ws/src/uav/launch/launch_params.yaml
+++ b/controls/sae_2025_ws/src/uav/launch/launch_params.yaml
@@ -1,5 +1,5 @@
 # launch_params.yaml
-mission_name: basic
+mission_name: mission
 uav_debug: false
 vision_debug: false
 sim: true
diff --git a/controls/sae_2025_ws/src/uav/launch/main.launch.py b/controls/sae_2025_ws/src/uav/launch/main.launch.py
index 78f849d8..04b70102 100644
--- a/controls/sae_2025_ws/src/uav/launch/main.launch.py
+++ b/controls/sae_2025_ws/src/uav/launch/main.launch.py
@@ -9,8 +9,8 @@
 from launch_ros.actions import Node
 from uav.utils import vehicle_map
 
-GZ_CAMERA_TOPIC = '/world/custom/model/x500_mono_cam_down_0/link/camera_link/sensor/imager/image'
-GZ_CAMERA_INFO_TOPIC = '/world/custom/model/x500_mono_cam_down_0/link/camera_link/sensor/imager/camera_info'
+GZ_CAMERA_TOPIC = '/world/custom/model/x500_mono_cam_down_0/link/camera_link/sensor/camera/image'
+GZ_CAMERA_INFO_TOPIC = '/world/custom/model/x500_mono_cam_down_0/link/camera_link/sensor/camera/camera_info'
 HARDCODE_PATH = False
 
 def find_folder(folder_name, search_path):
@@ -158,7 +158,7 @@ def launch_setup(context, *args, **kwargs):
     else:
         raise ValueError(f"Invalid vehicle type: {vehicle_type}")
     px4_sitl = ExecuteProcess(
-        cmd=['bash', '-c', f'PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART={autostart} PX4_SIM_MODEL={model} PX4_GZ_WORLD=custom ./build/px4_sitl_default/bin/px4'],
+        cmd=['bash', '-c', f'PX4_GZ_STANDALONE=1 PX4_SYS_AUTOSTART={autostart} PX4_SIM_MODEL={model} ./build/px4_sitl_default/bin/px4'],
         cwd=px4_path,
         output='screen',
         name='px4_sitl'
@@ -168,7 +168,7 @@ def launch_setup(context, *args, **kwargs):
     gz_ros_bridge_camera = ExecuteProcess(
         cmd=['ros2', 'run', 'ros_gz_bridge', 'parameter_bridge',
             f'{GZ_CAMERA_TOPIC}@sensor_msgs/msg/Image[gz.msgs.Image',
-            '--ros-args', '--remap', '/world/custom/model/x500_mono_cam_down_0/link/camera_link/sensor/imager/image:=/camera'],
+            '--ros-args', '--remap', f'{GZ_CAMERA_TOPIC}:=/camera'],
         output='screen',
         cwd=sae_ws_path,
         name='gz_ros_bridge_camera'
@@ -177,7 +177,7 @@ def launch_setup(context, *args, **kwargs):
     gz_ros_bridge_camera_info = ExecuteProcess(
         cmd=['ros2', 'run', 'ros_gz_bridge', 'parameter_bridge',
             f'{GZ_CAMERA_INFO_TOPIC}@sensor_msgs/msg/CameraInfo[gz.msgs.CameraInfo',
-            '--ros-args', '--remap', '/world/custom/model/x500_mono_cam_down_0/link/camera_link/sensor/imager/camera_info:=/camera_info'],
+            '--ros-args', '--remap', f'{GZ_CAMERA_INFO_TOPIC}:=/camera_info'],
         output='screen',
         cwd=sae_ws_path,
         name='gz_ros_bridge_camera_info'
diff --git a/controls/sae_2025_ws/src/uav/uav/UAV.py b/controls/sae_2025_ws/src/uav/uav/UAV.py
index 6a28ed86..687ff65a 100755
--- a/controls/sae_2025_ws/src/uav/uav/UAV.py
+++ b/controls/sae_2025_ws/src/uav/uav/UAV.py
@@ -471,7 +471,7 @@ def _initialize_publishers_and_subscribers(self):
         # Subscribers
         self.status_sub = self.node.create_subscription(
             VehicleStatus,
-            '/fmu/out/vehicle_status',
+            '/fmu/out/vehicle_status_v1',
             self._vehicle_status_callback,
             qos_profile
         )