diff --git a/config/gazebo-iris-gimbal.parm b/config/gazebo-iris-gimbal.parm
index c2d29622..dd36c19d 100644
--- a/config/gazebo-iris-gimbal.parm
+++ b/config/gazebo-iris-gimbal.parm
@@ -2,17 +2,18 @@
 FRAME_CLASS	     1
 FRAME_TYPE	     1
 
-# IRLOCK FEATURE
-RC8_OPTION       39
-PLND_ENABLED     1
-PLND_TYPE        3
+# Match servo out for motors
+MOT_PWM_MIN      1100
+MOT_PWM_MAX      1900
 
-# SONAR FOR IRLOCK
-SIM_SONAR_SCALE  10
-RNGFND1_TYPE     1
-RNGFND1_SCALING  10
-RNGFND1_PIN      0
-RNGFND1_MAX_CM   5000
+# Gimbal on mount 1 has 3 DOF
+MNT1_TYPE        1           # Servo
+MNT1_PITCH_MAX   45
+MNT1_PITCH_MIN   -135
+MNT1_ROLL_MAX    30
+MNT1_ROLL_MIN    -30
+MNT1_YAW_MAX     160
+MNT1_YAW_MIN     -160
 
 # Gimbal RC in
 RC6_MAX          1900
diff --git a/models/gimbal_small_1d/model.sdf b/models/gimbal_small_1d/model.sdf
index f449fae6..4fefeb56 100644
--- a/models/gimbal_small_1d/model.sdf
+++ b/models/gimbal_small_1d/model.sdf
@@ -140,11 +140,11 @@
       <axis>
         <xyz>1 0 0</xyz>
         <dynamics>
-          <damping>0.5</damping>
+          <damping>0.01</damping>
         </dynamics>
         <limit>
-          <lower>-1.57079632</lower>
-          <upper>1.57079632</upper>
+          <lower>-3.1415926</lower>
+          <upper>3.1415926</upper>
         </limit>
       </axis>
       <pose>0 0 0.02 0 0 0</pose>
diff --git a/models/gimbal_small_2d/model.sdf b/models/gimbal_small_2d/model.sdf
index 794e9980..f8db885a 100644
--- a/models/gimbal_small_2d/model.sdf
+++ b/models/gimbal_small_2d/model.sdf
@@ -76,11 +76,11 @@
       <axis>
         <xyz>0 0 1</xyz>
         <dynamics>
-          <damping>0.5</damping>
+          <damping>0.01</damping>
         </dynamics>
         <limit>
-          <lower>-1.57079632</lower>
-          <upper>1.57079632</upper>
+          <lower>-3.1415926</lower>
+          <upper>3.1415926</upper>
         </limit>
       </axis>
       <pose>0.01 0 -0.04 0 0 0</pose>
@@ -176,11 +176,11 @@
       <axis>
         <xyz>1 0 0</xyz>
         <dynamics>
-          <damping>0.5</damping>
+          <damping>0.01</damping>
         </dynamics>
         <limit>
-          <lower>-1.57079632</lower>
-          <upper>1.57079632</upper>
+          <lower>-3.1415926</lower>
+          <upper>3.1415926</upper>
         </limit>
       </axis>
       <pose>0 0 0.02 0 0 0</pose>
diff --git a/models/gimbal_small_3d/model.sdf b/models/gimbal_small_3d/model.sdf
index d265f7ea..0c4b0145 100644
--- a/models/gimbal_small_3d/model.sdf
+++ b/models/gimbal_small_3d/model.sdf
@@ -1,8 +1,8 @@
 <?xml version="1.0"?>
 <sdf version="1.9">
   <model name="gimbal_small_3d">
-    <pose>0 0 0.18 0 0 0</pose>
-    <link name="base_link">
+    <!-- <pose>0 0 0.18 0 0 0</pose> -->
+    <link name="gimbal_link">
       <inertial>
         <mass>0.2</mass>
         <inertia>
@@ -14,10 +14,10 @@
           <izz>0.0001</izz>
         </inertia>
       </inertial>
-      <visual name="base_visual">
+      <visual name="gimbal_visual">
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/base_plate.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/base_plate.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -26,7 +26,7 @@
           <specular>0.01 0.01 0.01 1.0</specular>
         </material>
       </visual>
-      <collision name="base_collision">
+      <collision name="gimbal_collision">
         <pose>0.01 0.075 -0.025 0 0 0</pose>
         <geometry>
           <box>
@@ -51,7 +51,7 @@
       <visual name="yaw_visual">
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/yaw_arm.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/yaw_arm.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -63,22 +63,22 @@
       <collision name="yaw_collision">
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/yaw_arm.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/yaw_arm.dae</uri>
           </mesh>
         </geometry>
       </collision>
     </link>
     <joint name="yaw_joint" type="revolute">
-      <parent>base_link</parent>
+      <parent>gimbal_link</parent>
       <child>yaw_link</child>
       <axis>
         <xyz>0 1 0</xyz>
         <dynamics>
-          <damping>0.5</damping>
+          <damping>0.01</damping>
         </dynamics>
         <limit>
-          <lower>-1.57079632</lower>
-          <upper>1.57079632</upper>
+          <lower>-3.1415926</lower>
+          <upper>3.1415926</upper>
         </limit>
       </axis>
       <pose>0.0105 0.065 -0.002 0 0 0</pose>
@@ -99,7 +99,7 @@
       <visual name="roll_visual">
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/roll_arm.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/roll_arm.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -111,7 +111,7 @@
       <collision name="roll_collision">
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/roll_arm.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/roll_arm.dae</uri>
           </mesh>
         </geometry>
       </collision>
@@ -122,11 +122,11 @@
       <axis>
         <xyz>0 0 1</xyz>
         <dynamics>
-          <damping>0.5</damping>
+          <damping>0.01</damping>
         </dynamics>
         <limit>
-          <lower>-1.57079632</lower>
-          <upper>1.57079632</upper>
+          <lower>-3.1415926</lower>
+          <upper>3.1415926</upper>
         </limit>
       </axis>
       <pose>0.0099 0.002 -0.05 0 0 0</pose>
@@ -148,7 +148,7 @@
         <pose>0 0 0 0 0 0</pose>
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/camera_enclosure.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/camera_enclosure.dae</uri>
           </mesh>
         </geometry>
         <material>
@@ -161,7 +161,7 @@
         <pose>0 0 0 0 0 0</pose>
         <geometry>
           <mesh>
-            <uri>model://gimbal_small_3d/meshes/camera_enclosure.dae</uri>
+            <uri>package://ardupilot_gazebo/models/gimbal_small_3d/meshes/camera_enclosure.dae</uri>
           </mesh>
         </geometry>
       </collision>
@@ -227,11 +227,11 @@
       <axis>
         <xyz>1 0 0</xyz>
         <dynamics>
-          <damping>0.5</damping>
+          <damping>0.01</damping>
         </dynamics>
         <limit>
-          <lower>-1.57079632</lower>
-          <upper>1.57079632</upper>
+          <lower>-3.1415926</lower>
+          <upper>3.1415926</upper>
         </limit>
       </axis>
       <pose>0.045 0.0021 0.0199 0 0 0</pose>
diff --git a/models/iris_with_gimbal/model.sdf b/models/iris_with_gimbal/model.sdf
index 0840b239..a313a643 100755
--- a/models/iris_with_gimbal/model.sdf
+++ b/models/iris_with_gimbal/model.sdf
@@ -7,9 +7,9 @@
     </include>
 
     <include merge="true">
-      <uri>package://ardupilot_gazebo/models/gimbal_small_2d</uri>
+      <uri>package://ardupilot_gazebo/models/gimbal_small_3d</uri>
       <name>gimbal</name>
-      <pose degrees="true">0 -0.01 -0.124923 90 0 90</pose>    
+      <pose degrees="true">0 -0.01 -0.124923 90 0 90</pose>
     </include>
 
     <joint name="gimbal_joint" type="revolute">
@@ -194,21 +194,6 @@
       <joint_name>rotor_3_joint</joint_name>
     </plugin>
 
-    <plugin
-      filename="gz-sim-joint-position-controller-system"
-      name="gz::sim::systems::JointPositionController">
-      <joint_name>roll_joint</joint_name>
-      <topic>/gimbal/cmd_roll</topic>
-      <p_gain>2</p_gain>
-    </plugin>
-    <plugin
-      filename="gz-sim-joint-position-controller-system"
-      name="gz::sim::systems::JointPositionController">
-      <joint_name>tilt_joint</joint_name>
-      <topic>/gimbal/cmd_tilt</topic>
-      <p_gain>2</p_gain>
-    </plugin>
-
     <plugin filename="gz-sim-linearbatteryplugin-system"
       name="gz::sim::systems::LinearBatteryPlugin">
       <battery_name>lipo_3500mAh</battery_name>
@@ -224,6 +209,7 @@
 
     <plugin name="ArduPilotPlugin"
       filename="ArduPilotPlugin">
+      <!-- Port settings -->
       <fdm_addr>127.0.0.1</fdm_addr>
       <fdm_port_in>9002</fdm_port_in>
       <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
@@ -238,7 +224,6 @@
       <!-- Sensors -->
       <imuName>imu_link::imu_sensor</imuName>
 
-      <!-- Control channels -->
       <!--
           incoming control command [0, 1]
           so offset it by 0 to get [0, 1]
@@ -318,29 +303,65 @@
         <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
       </control>
 
-      <control channel="4">
+      <!-- roll range is -30 to +30 deg -->
+      <control channel="8">
         <jointName>roll_joint</jointName>
-        <multiplier>3.14159265</multiplier>
+        <multiplier>1.047197551196</multiplier>
         <offset>-0.5</offset>
         <servo_min>1100</servo_min>
         <servo_max>1900</servo_max>
         <type>COMMAND</type>
         <cmd_topic>/gimbal/cmd_roll</cmd_topic>
-        <p_gain>3</p_gain>
+        <p_gain>2</p_gain>
+      </control>
+
+      <!-- pitch range is -135 to +45 deg -->
+      <control channel="9">
+        <jointName>pitch_joint</jointName>
+        <multiplier>-3.14159265</multiplier>
+        <offset>-0.75</offset>
+        <servo_min>1100</servo_min>
+        <servo_max>1900</servo_max>
+        <type>COMMAND</type>
+        <cmd_topic>/gimbal/cmd_pitch</cmd_topic>
+        <p_gain>2</p_gain>
       </control>
 
-      <control channel="5">
-        <jointName>tilt_joint</jointName>
-        <multiplier>3.14159265</multiplier>
+      <!-- yaw range is -160 to +160 deg -->
+      <control channel="10">
+        <jointName>yaw_joint</jointName>
+        <multiplier>-5.5850536</multiplier>
         <offset>-0.5</offset>
         <servo_min>1100</servo_min>
         <servo_max>1900</servo_max>
         <type>COMMAND</type>
-        <cmd_topic>/gimbal/cmd_tilt</cmd_topic>
-        <p_gain>3</p_gain>
+        <cmd_topic>/gimbal/cmd_yaw</cmd_topic>
+        <p_gain>2</p_gain>
       </control>
 
     </plugin>
 
+    <plugin
+      filename="gz-sim-joint-position-controller-system"
+      name="gz::sim::systems::JointPositionController">
+      <joint_name>roll_joint</joint_name>
+      <topic>/gimbal/cmd_roll</topic>
+      <p_gain>2</p_gain>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system"
+      name="gz::sim::systems::JointPositionController">
+      <joint_name>pitch_joint</joint_name>
+      <topic>/gimbal/cmd_pitch</topic>
+      <p_gain>2</p_gain>
+    </plugin>
+    <plugin
+      filename="gz-sim-joint-position-controller-system"
+      name="gz::sim::systems::JointPositionController">
+      <joint_name>yaw_joint</joint_name>
+      <topic>/gimbal/cmd_yaw</topic>
+      <p_gain>2</p_gain>
+    </plugin>
+
   </model>
 </sdf>
diff --git a/worlds/iris_runway.sdf b/worlds/iris_runway.sdf
index 8f87d99e..259ac716 100755
--- a/worlds/iris_runway.sdf
+++ b/worlds/iris_runway.sdf
@@ -116,7 +116,7 @@
     </include>
 
     <include>
-      <uri>model://iris_with_ardupilot</uri>
+      <uri>model://iris_with_gimbal</uri>
       <pose degrees="true">0 0 0.195 0 0 90</pose>
     </include>
 
diff --git a/worlds/iris_warehouse.sdf b/worlds/iris_warehouse.sdf
index 80010aca..a93cc61c 100644
--- a/worlds/iris_warehouse.sdf
+++ b/worlds/iris_warehouse.sdf
@@ -250,7 +250,7 @@
 
     <include>
         <pose>-6 0 0.25 0 0 0</pose>
-        <uri>model://iris_with_ardupilot</uri>
+        <uri>model://iris_with_gimbal</uri>
     </include>
 
     <include>
diff --git a/worlds/zephyr_parachute.sdf b/worlds/zephyr_parachute.sdf
index 75fc1ddd..32899f18 100755
--- a/worlds/zephyr_parachute.sdf
+++ b/worlds/zephyr_parachute.sdf
@@ -128,14 +128,6 @@
       <surface_model>EARTH_WGS84</surface_model>
     </spherical_coordinates>
 
-    <spherical_coordinates>
-      <latitude_deg>-35.363262</latitude_deg>
-      <longitude_deg>149.165237</longitude_deg>
-      <elevation>584</elevation>
-      <heading_deg>0</heading_deg>
-      <surface_model>EARTH_WGS84</surface_model>
-    </spherical_coordinates>
-
     <light type="directional" name="sun">
       <cast_shadows>true</cast_shadows>
       <pose>0 0 10 0 0 0</pose>
@@ -150,13 +142,69 @@
       <direction>-0.5 0.1 -0.9</direction>
     </light>
 
+    <model name="axes">
+      <static>1</static>
+      <link name="link">
+        <visual name="r">
+          <cast_shadows>0</cast_shadows>
+          <pose>5 0 0.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>10 0.01 0.01</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>1 0 0 0.8</ambient>
+            <diffuse>1 0 0 0.8</diffuse>
+            <emissive>1 0 0 0.8</emissive>
+            <specular>0.5 0.5 0.5 0.8</specular>
+          </material>
+        </visual>
+        <visual name="g">
+          <cast_shadows>0</cast_shadows>
+          <pose>0 5 0.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.01 10 0.01</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0 1 0 0.8</ambient>
+            <diffuse>0 1 0 0.8</diffuse>
+            <emissive>0 1 0 0.8</emissive>
+            <specular>0.5 0.5 0.5 0.8</specular>
+          </material>
+        </visual>
+        <visual name="b">
+          <cast_shadows>0</cast_shadows>
+          <pose>0 0 5.1 0 0 0</pose>
+          <geometry>
+            <box>
+              <size>0.01 0.01 10</size>
+            </box>
+          </geometry>
+          <material>
+            <ambient>0 0 1 0.8</ambient>
+            <diffuse>0 0 1 0.8</diffuse>
+            <emissive>0 0 1 0.8</emissive>
+            <specular>0.5 0.5 0.5 0.8</specular>
+          </material>
+        </visual>
+        <sensor name="navsat_sensor" type="navsat">
+          <always_on>1</always_on>
+          <update_rate>1</update_rate>
+        </sensor>
+      </link>
+    </model>
+    
     <include>
       <uri>model://runway</uri>
+      <pose degrees="true">-29 545 0 0 0 363</pose>
     </include>
 
     <include>
-      <pose>0 0 0.422 -1.5707963 0 1.5707963</pose>
       <uri>model://zephyr_with_parachute</uri>
+      <pose degrees="true">0 0 0.422 -90 0 180</pose>
 
       <plugin name="ParachutePlugin" filename="ParachutePlugin">
         <parent_link>parachute_attachment_link</parent_link>
@@ -170,4 +218,3 @@
 
   </world>
 </sdf>
-