<!-- shared control loops gain -->

<airframe>

  <commands>
    <axis name="PITCH" failsafe_value="0"/>
    <axis name="ROLL" failsafe_value="0"/>
    <axis name="YAW" failsafe_value="0"/>
    <axis name="THRUST" failsafe_value="0"/>
  </commands>

  <servos driver="Default">
    <servo name="TOP_LEFT" no="0" min="0" neutral="1" max="500"/>
    <servo name="TOP_RIGHT" no="1" min="0" neutral="1" max="500"/>
    <servo name="BOTTOM_RIGHT" no="2" min="0" neutral="1" max="500"/>
    <servo name="BOTTOM_LEFT" no="3" min="0" neutral="1" max="500"/>
  </servos>

  <section name="MIXING" prefix="MOTOR_MIXING_">
    <define name="TRIM_ROLL" value="0"/>
    <define name="TRIM_PITCH" value="-200"/>
    <define name="TRIM_YAW" value="0"/>
    <define name="NB_MOTOR" value="4"/>
    <define name="SCALE" value="255"/>

    <!-- Time cross layout (X), with order NW (CW), NE (CCW), SE (CW), SW (CCW) -->
    <define name="ROLL_COEF" value="{   256, -256, -256,  256 }"/>
    <define name="PITCH_COEF" value="{  256,  256, -256, -256 }"/>
    <define name="YAW_COEF" value="{   -256,  256, -256,  256 }"/>
    <define name="THRUST_COEF" value="{ 256,  256,  256,  256 }"/>
  </section>

  <command_laws>
    <call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
    <set servo="TOP_LEFT" value="motor_mixing.commands[0]"/>
    <set servo="TOP_RIGHT" value="motor_mixing.commands[1]"/>
    <set servo="BOTTOM_RIGHT" value="motor_mixing.commands[2]"/>
    <set servo="BOTTOM_LEFT" value="motor_mixing.commands[3]"/>
  </command_laws>

  <section name="INS_BASE" prefix="INS_">
    <define name="SONAR_UPDATE_ON_AGL" value="TRUE"/>
    <define name="SONAR_MAX_RANGE" value="4.0"/>
    <define name="SONAR_MIN_RANGE" value="0.01"/>
  </section>


  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
    <!-- setpoints -->
    <define name="SP_MAX_PHI" value="45" unit="deg"/>
    <define name="SP_MAX_THETA" value="45" unit="deg"/>
    <define name="SP_MAX_R" value="200" unit="deg/s"/>
    <define name="DEADBAND_A" value="0"/>
    <define name="DEADBAND_E" value="0"/>
    <define name="DEADBAND_R" value="250"/>

    <!-- reference -->
    <define name="REF_OMEGA_P" value="900" unit="deg/s"/>
    <define name="REF_ZETA_P" value="0.9"/>
    <define name="REF_MAX_P" value="600." unit="deg/s"/>
    <define name="REF_MAX_PDOT" value="RadOfDeg(8000.)"/>

    <define name="REF_OMEGA_Q" value="900" unit="deg/s"/>
    <define name="REF_ZETA_Q" value="0.9"/>
    <define name="REF_MAX_Q" value="600." unit="deg/s"/>
    <define name="REF_MAX_QDOT" value="RadOfDeg(8000.)"/>

    <define name="REF_OMEGA_R" value="600" unit="deg/s"/>
    <define name="REF_ZETA_R" value="0.9"/>
    <define name="REF_MAX_R" value="40." unit="deg/s"/>
    <define name="REF_MAX_RDOT" value="RadOfDeg(4000.)"/>

    <!-- feedback -->
    <define name="PHI_PGAIN" value="1100"/>
    <define name="PHI_DGAIN" value="420"/>
    <define name="PHI_IGAIN" value="120"/>

    <define name="THETA_PGAIN" value="1100"/>
    <define name="THETA_DGAIN" value="420"/>
    <define name="THETA_IGAIN" value="120"/>

    <define name="PSI_PGAIN" value="1000"/>
    <define name="PSI_DGAIN" value="400"/>
    <define name="PSI_IGAIN" value="30"/>

    <!-- feedforward -->
    <define name="PHI_DDGAIN" value="70"/>
    <define name="THETA_DDGAIN" value="70"/>
    <define name="PSI_DDGAIN" value="50"/>
  </section>

  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
    <define name="MIN_ERR_Z" value="POS_BFP_OF_REAL(-10.)"/>
    <define name="MAX_ERR_Z" value="POS_BFP_OF_REAL( 10.)"/>
    <define name="MIN_ERR_ZD" value="SPEED_BFP_OF_REAL(-10.)"/>
    <define name="MAX_ERR_ZD" value="SPEED_BFP_OF_REAL( 10.)"/>
    <define name="MAX_SUM_ERR" value="2000000"/>
    <define name="REF_MIN_ZDD"  value="-0.8*9.81"/>
    <define name="REF_MAX_ZDD"  value=" 0.5*9.81"/>
    <define name="REF_MIN_ZD"   value="-1.5"/>
    <define name="REF_MAX_ZD"   value=" 1.5"/>
    <define name="HOVER_KP" value="200"/>
    <define name="HOVER_KD" value="100"/>
    <define name="HOVER_KI" value="30"/>
    <define name="NOMINAL_HOVER_THROTTLE" value="0.65"/>
    <define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
    <define name="ADAPT_INITIAL_HOVER_THROTTLE" value="0.5"/>
  </section>

  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
    <define name="REF_MAX_SPEED" value="3." unit="m/s"/>
    <!-- Good weather -->
    <define name="MAX_BANK" value="20" unit="deg"/>
    <!-- Bad weather -->
    <!-- define name="MAX_BANK" value="32" unit="deg"/ -->
    <define name="PGAIN" value="40"/>
    <define name="DGAIN" value="60"/>
    <define name="IGAIN" value="20"/>
  </section>

  <section name="SIMULATOR" prefix="NPS_">
    <define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
    <define name="JSBSIM_MODEL" value="simple_ardrone2" type="string"/>
    <define name="SENSORS_PARAMS" value="nps_sensors_params_invariant.h" type="string"/>
  </section>

  <section name="AUTOPILOT">
    <define name="MODE_STARTUP" value="AP_MODE_NAV"/>
    <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
    <define name="MODE_AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD"/>
    <define name="MODE_AUTO2" value="AP_MODE_NAV"/>
  </section>

  <section name="BAT">
    <define name="MILLIAMP_AT_FULL_THROTTLE" value="8700"/>
    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
    <define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/>
    <define name="LOW_BAT_LEVEL" value="9.7" unit="V"/>
    <define name="MAX_BAT_LEVEL" value="12.4" unit="V"/>
  </section>

</airframe>
