<?xml version='1.0'?>
<sdf version="1.9">
  <model name="iris_with_gimbal">
    <include>
      <uri>model://iris_with_standoffs</uri>
    </include>

    <include>
      <uri>model://gimbal_small_3d</uri>
      <name>gimbal</name>
      <pose degrees="true">0 -0.01 -0.124923 90 0 90</pose>    
    </include>

    <joint name="gimbal_joint" type="revolute">
      <parent>iris_with_standoffs::base_link</parent>
      <child>gimbal::base_link</child>
      <axis>
        <limit>
          <lower>0</lower>
          <upper>0</upper>
        </limit>
        <xyz>0 0 1</xyz>
      </axis>
    </joint>

    <!-- plugins -->
    <plugin filename="gz-sim-joint-state-publisher-system"
      name="gz::sim::systems::JointStatePublisher">
    </plugin>
    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.3</a0>
      <alpha_stall>1.4</alpha_stall>
      <cla>4.2500</cla>
      <cda>0.10</cda>
      <cma>0.0</cma>
      <cla_stall>-0.025</cla_stall>
      <cda_stall>0.0</cda_stall>
      <cma_stall>0.0</cma_stall>
      <area>0.002</area>
      <air_density>1.2041</air_density>
      <cp>0.084 0 0</cp>
      <forward>0 1 0</forward>
      <upward>0 0 1</upward>
      <link_name>iris_with_standoffs::rotor_0</link_name>
    </plugin>
    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.3</a0>
      <alpha_stall>1.4</alpha_stall>
      <cla>4.2500</cla>
      <cda>0.10</cda>
      <cma>0.0</cma>
      <cla_stall>-0.025</cla_stall>
      <cda_stall>0.0</cda_stall>
      <cma_stall>0.0</cma_stall>
      <area>0.002</area>
      <air_density>1.2041</air_density>
      <cp>-0.084 0 0</cp>
      <forward>0 -1 0</forward>
      <upward>0 0 1</upward>
      <link_name>iris_with_standoffs::rotor_0</link_name>
    </plugin>

    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.3</a0>
      <alpha_stall>1.4</alpha_stall>
      <cla>4.2500</cla>
      <cda>0.10</cda>
      <cma>0.0</cma>
      <cla_stall>-0.025</cla_stall>
      <cda_stall>0.0</cda_stall>
      <cma_stall>0.0</cma_stall>
      <area>0.002</area>
      <air_density>1.2041</air_density>
      <cp>0.084 0 0</cp>
      <forward>0 1 0</forward>
      <upward>0 0 1</upward>
      <link_name>iris_with_standoffs::rotor_1</link_name>
    </plugin>
    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.3</a0>
      <alpha_stall>1.4</alpha_stall>
      <cla>4.2500</cla>
      <cda>0.10</cda>
      <cma>0.0</cma>
      <cla_stall>-0.025</cla_stall>
      <cda_stall>0.0</cda_stall>
      <cma_stall>0.0</cma_stall>
      <area>0.002</area>
      <air_density>1.2041</air_density>
      <cp>-0.084 0 0</cp>
      <forward>0 -1 0</forward>
      <upward>0 0 1</upward>
      <link_name>iris_with_standoffs::rotor_1</link_name>
    </plugin>

    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.3</a0>
      <alpha_stall>1.4</alpha_stall>
      <cla>4.2500</cla>
      <cda>0.10</cda>
      <cma>0.0</cma>
      <cla_stall>-0.025</cla_stall>
      <cda_stall>0.0</cda_stall>
      <cma_stall>0.0</cma_stall>
      <area>0.002</area>
      <air_density>1.2041</air_density>
      <cp>0.084 0 0</cp>
      <forward>0 -1 0</forward>
      <upward>0 0 1</upward>
      <link_name>iris_with_standoffs::rotor_2</link_name>
    </plugin>
    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.3</a0>
      <alpha_stall>1.4</alpha_stall>
      <cla>4.2500</cla>
      <cda>0.10</cda>
      <cma>0.0</cma>
      <cla_stall>-0.025</cla_stall>
      <cda_stall>0.0</cda_stall>
      <cma_stall>0.0</cma_stall>
      <area>0.002</area>
      <air_density>1.2041</air_density>
      <cp>-0.084 0 0</cp>
      <forward>0 1 0</forward>
      <upward>0 0 1</upward>
      <link_name>iris_with_standoffs::rotor_2</link_name>
    </plugin>

    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.3</a0>
      <alpha_stall>1.4</alpha_stall>
      <cla>4.2500</cla>
      <cda>0.10</cda>
      <cma>0.0</cma>
      <cla_stall>-0.025</cla_stall>
      <cda_stall>0.0</cda_stall>
      <cma_stall>0.0</cma_stall>
      <area>0.002</area>
      <air_density>1.2041</air_density>
      <cp>0.084 0 0</cp>
      <forward>0 -1 0</forward>
      <upward>0 0 1</upward>
      <link_name>iris_with_standoffs::rotor_3</link_name>
    </plugin>
    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.3</a0>
      <alpha_stall>1.4</alpha_stall>
      <cla>4.2500</cla>
      <cda>0.10</cda>
      <cma>0.0</cma>
      <cla_stall>-0.025</cla_stall>
      <cda_stall>0.0</cda_stall>
      <cma_stall>0.0</cma_stall>
      <area>0.002</area>
      <air_density>1.2041</air_density>
      <cp>-0.084 0 0</cp>
      <forward>0 1 0</forward>
      <upward>0 0 1</upward>
      <link_name>iris_with_standoffs::rotor_3</link_name>
    </plugin>

    <plugin filename="gz-sim-apply-joint-force-system"
      name="gz::sim::systems::ApplyJointForce">
      <joint_name>iris_with_standoffs::rotor_0_joint</joint_name>
    </plugin>
    <plugin filename="gz-sim-apply-joint-force-system"
      name="gz::sim::systems::ApplyJointForce">
      <joint_name>iris_with_standoffs::rotor_1_joint</joint_name>
    </plugin>
    <plugin filename="gz-sim-apply-joint-force-system"
      name="gz::sim::systems::ApplyJointForce">
      <joint_name>iris_with_standoffs::rotor_2_joint</joint_name>
    </plugin>
    <plugin filename="gz-sim-apply-joint-force-system"
      name="gz::sim::systems::ApplyJointForce">
      <joint_name>iris_with_standoffs::rotor_3_joint</joint_name>
    </plugin>

    <plugin name="ArduPilotPlugin"
      filename="ArduPilotPlugin">
      <!-- Port settings -->
      <fdm_addr>0.0.0.0</fdm_addr>
      <fdm_port_in>9002</fdm_port_in>
      <connectionTimeoutMaxCount>5</connectionTimeoutMaxCount>
      <lock_step>1</lock_step>

      <!-- Frame conventions
        Require by ArduPilot: change model and gazebo from XYZ to XY-Z coordinates
      -->
      <modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 0</modelXYZToAirplaneXForwardZDown>
      <gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>

      <!-- Sensors -->
      <imuName>iris_with_standoffs::imu_link::imu_sensor</imuName>

      <!--
          incoming control command [0, 1]
          so offset it by 0 to get [0, 1]
          and divide max target by 1.
          offset = 0
          multiplier = 838 max rpm / 1 = 838
        -->
      <control channel="0">
        <jointName>iris_with_standoffs::rotor_0_joint</jointName>
        <useForce>1</useForce>
        <multiplier>838</multiplier>
        <offset>0</offset>
        <servo_min>1100</servo_min>
        <servo_max>1900</servo_max>
        <type>VELOCITY</type>
        <p_gain>0.20</p_gain>
        <i_gain>0</i_gain>
        <d_gain>0</d_gain>
        <i_max>0</i_max>
        <i_min>0</i_min>
        <cmd_max>2.5</cmd_max>
        <cmd_min>-2.5</cmd_min>
        <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
      </control>

      <control channel="1">
        <jointName>iris_with_standoffs::rotor_1_joint</jointName>
        <useForce>1</useForce>
        <multiplier>838</multiplier>
        <offset>0</offset>
        <servo_min>1100</servo_min>
        <servo_max>1900</servo_max>
        <type>VELOCITY</type>
        <p_gain>0.20</p_gain>
        <i_gain>0</i_gain>
        <d_gain>0</d_gain>
        <i_max>0</i_max>
        <i_min>0</i_min>
        <cmd_max>2.5</cmd_max>
        <cmd_min>-2.5</cmd_min>
        <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
      </control>

      <control channel="2">
        <jointName>iris_with_standoffs::rotor_2_joint</jointName>
        <useForce>1</useForce>
        <multiplier>-838</multiplier>
        <offset>0</offset>
        <servo_min>1100</servo_min>
        <servo_max>1900</servo_max>
        <type>VELOCITY</type>
        <p_gain>0.20</p_gain>
        <i_gain>0</i_gain>
        <d_gain>0</d_gain>
        <i_max>0</i_max>
        <i_min>0</i_min>
        <cmd_max>2.5</cmd_max>
        <cmd_min>-2.5</cmd_min>
        <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
      </control>

      <control channel="3">
        <jointName>iris_with_standoffs::rotor_3_joint</jointName>
        <useForce>1</useForce>
        <multiplier>-838</multiplier>
        <offset>0</offset>
        <servo_min>1100</servo_min>
        <servo_max>1900</servo_max>
        <type>VELOCITY</type>
        <p_gain>0.20</p_gain>
        <i_gain>0</i_gain>
        <d_gain>0</d_gain>
        <i_max>0</i_max>
        <i_min>0</i_min>
        <cmd_max>2.5</cmd_max>
        <cmd_min>-2.5</cmd_min>
        <controlVelocitySlowdownSim>1</controlVelocitySlowdownSim>
      </control>

      <!-- roll range is -30 to +30 deg -->
      <control channel="8">
        <jointName>gimbal::roll_joint</jointName>
        <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>2</p_gain>
      </control>

      <!-- pitch range is -135 to +45 deg -->
      <control channel="9">
        <jointName>gimbal::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>

      <!-- yaw range is -160 to +160 deg -->
      <control channel="10">
        <jointName>gimbal::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_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>gimbal::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>gimbal::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>gimbal::yaw_joint</joint_name>
      <topic>/gimbal/cmd_yaw</topic>
      <p_gain>2</p_gain>
    </plugin>

  </model>
</sdf>
