<?xml version="1.0" ?>
<sdf version="1.9">
  <model name="zephyr_with_ardupilot">
    <include>
      <uri>model://zephyr</uri>
    </include>

    <!-- plugins -->
    <plugin filename="gz-sim-joint-state-publisher-system"
      name="gz::sim::systems::JointStatePublisher">
    </plugin>
    <!-- wing -->
    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.13</a0>
      <cla>3.7</cla>
      <cda>0.06417112299</cda>
      <cma>0.0</cma>
      <alpha_stall>0.3391428111</alpha_stall>
      <cla_stall>-3.85</cla_stall>
      <cda_stall>-0.9233984055</cda_stall>
      <cma_stall>0.0</cma_stall>
      <cp>0 -0.1 0</cp>
      <area>0.50</area>
      <air_density>1.2041</air_density>
      <forward>0 -1 0</forward>
      <upward>0 0 1</upward>
      <link_name>zephyr::wing</link_name>
    </plugin>
    <!-- left_wing -->
    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.15</a0>
      <cla>6.8</cla>
      <cda>0.06417112299</cda>
      <cma>0.0</cma>
      <alpha_stall>0.6391428111</alpha_stall>
      <cla_stall>-3.85</cla_stall>
      <cda_stall>-0.9233984055</cda_stall>
      <cma_stall>0.0</cma_stall>
      <cp>0.7 0.20 0</cp>
      <area>0.10</area>
      <air_density>1.2041</air_density>
      <forward>0 -1 0</forward>
      <upward>0 0 1</upward>
      <link_name>zephyr::wing</link_name>
      <control_joint_name>zephyr::flap_left_joint</control_joint_name>
      <control_joint_rad_to_cl>-5.0</control_joint_rad_to_cl>
    </plugin>
    <!-- right_wing -->
    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.15</a0>
      <cla>6.8</cla>
      <cda>0.06417112299</cda>
      <cma>0.0</cma>
      <alpha_stall>0.6391428111</alpha_stall>
      <cla_stall>-3.85</cla_stall>
      <cda_stall>-0.9233984055</cda_stall>
      <cma_stall>0.0</cma_stall>
      <cp>-0.7 0.20 0</cp>
      <area>0.10</area>
      <air_density>1.2041</air_density>
      <forward>0 -1 0</forward>
      <upward>0 0 1</upward>
      <link_name>zephyr::wing</link_name>
      <control_joint_name>zephyr::flap_right_joint</control_joint_name>
      <control_joint_rad_to_cl>-5.0</control_joint_rad_to_cl>
    </plugin>
    <!-- left_rudder -->
    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.0</a0>
      <cla>4.752798721</cla>
      <cda>0.6417112299</cda>
      <cma>0.0</cma>
      <alpha_stall>0.3391428111</alpha_stall>
      <cla_stall>-3.85</cla_stall>
      <cda_stall>-0.9233984055</cda_stall>
      <cma_stall>0.0</cma_stall>
      <cp>-0.76 0.30 0.025</cp>
      <area>0.12</area>
      <air_density>1.2041</air_density>
      <forward>0 -1 0</forward>
      <upward>1 0 0</upward>
      <link_name>zephyr::wing</link_name>
    </plugin>
    <!-- right_rudder -->
    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.0</a0>
      <cla>4.752798721</cla>
      <cda>0.6417112299</cda>
      <cma>0.0</cma>
      <alpha_stall>0.3391428111</alpha_stall>
      <cla_stall>-3.85</cla_stall>
      <cda_stall>-0.9233984055</cda_stall>
      <cma_stall>0.0</cma_stall>
      <cp>0.76 0.30 0.025</cp>
      <area>0.12</area>
      <air_density>1.2041</air_density>
      <forward>0 -1 0</forward>
      <upward>1 0 0</upward>
      <link_name>zephyr::wing</link_name>
    </plugin>
    <!-- propeller_blade_1 -->
    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.30</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.02</area>
      <air_density>1.2041</air_density>
      <cp>0 0 0.074205</cp>
      <forward>-1 0 0</forward>
      <upward>0 -1 0</upward>
      <link_name>zephyr::propeller</link_name>
    </plugin>
    <!-- propeller_blade_2 -->
    <plugin filename="gz-sim-lift-drag-system"
        name="gz::sim::systems::LiftDrag">
      <a0>0.30</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.02</area>
      <air_density>1.2041</air_density>
      <cp>0 0 -0.074205</cp>
      <forward>1 0 0</forward>
      <upward>0 -1 0</upward>
      <link_name>zephyr::propeller</link_name>
    </plugin>

    <plugin filename="gz-sim-apply-joint-force-system"
      name="gz::sim::systems::ApplyJointForce">
      <joint_name>zephyr::propeller_joint</joint_name>
    </plugin>
    <plugin filename="gz-sim-apply-joint-force-system"
      name="gz::sim::systems::ApplyJointForce">
      <joint_name>zephyr::flap_left_joint</joint_name>
    </plugin>
    <plugin filename="gz-sim-apply-joint-force-system"
      name="gz::sim::systems::ApplyJointForce">
      <joint_name>zephyr::flap_right_joint</joint_name>
    </plugin>

    <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>
      <lock_step>1</lock_step>

      <!-- Frame conventions
        modelXYZToAirplaneXForwardZDown:
          - transforms body frame from orientation in Gazebo to NED

        gazeboXYZToNED
          - transforms world from Gazebo convention xyz = N -E -D
            to ArduPilot convention xyz = NED

        Zephyr is oriented x-left, y-back, z-up
      -->
      <modelXYZToAirplaneXForwardZDown degrees="true">0 0 0 180 0 -90</modelXYZToAirplaneXForwardZDown>
      <gazeboXYZToNED degrees="true">0 0 0 180 0 90</gazeboXYZToNED>

      <!-- Sensors -->
      <imuName>zephyr::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
        -->

      <!--
          SERVO3_FUNCTION   70 (Throttle)
          SERVO3_MAX        1900
          SERVO3_MIN        1100
          SERVO3_REVERSED   0
          SERVO3_TRIM       1100
       -->
      <control channel="2">
        <jointName>zephyr::propeller_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>0.0</cmd_min>
      </control>

      <!--
          SERVO1_FUNCTION   77 (Elevon Left)
          SERVO1_MAX        1900
          SERVO1_MIN        1100
          SERVO1_REVERSED   1
          SERVO1_TRIM       1500

          pwm:          =>  [1100, 1900]
          input:        =>  [0, 1]
          offset: -0.5  =>  [-0.5, 0.5]
          scale:   2.0  =>  [-1.0, 1.0]
          scale: 0.524  =>  [-0.524, 0.524]
       -->
      <control channel="0">
        <jointName>zephyr::flap_left_joint</jointName>
        <useForce>1</useForce>
        <multiplier>-1.048</multiplier>
        <offset>-0.5</offset>
        <servo_min>1100</servo_min>
        <servo_max>1900</servo_max>
        <type>POSITION</type>
        <p_gain>10.0</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>
      </control>

      <!--
          SERVO2_FUNCTION   78 (Elevon Right)
          SERVO2_MAX        1900
          SERVO2_MIN        1100
          SERVO2_REVERSED   1
          SERVO2_TRIM       1500
       -->
      <control channel="1">
        <jointName>zephyr::flap_right_joint</jointName>
        <useForce>1</useForce>
        <multiplier>-1.048</multiplier>
        <offset>-0.5</offset>
        <servo_min>1100</servo_min>
        <servo_max>1900</servo_max>
        <type>POSITION</type>
        <p_gain>10.0</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>
      </control>

    </plugin>

  </model>
</sdf>
