<!-- this is a quadshot vehicle equiped with Lia -->
<!-- Copyright (C) 2012, Pranay Sinha, Piotr Esden-Tempski, Transition Robotics, Inc. -->

<airframe name="gain_scheduling_example">

  <servos driver="Pwm">
    <servo name="A1" no="0" min="1000" neutral="1000" max="2000"/>
    <servo name="A2" no="1" min="1000" neutral="1000" max="2000"/>
    <servo name="B1"  no="2" min="1000" neutral="1000" max="2000"/>
    <servo name="B2"  no="3" min="1000" neutral="1000" max="2000"/>
    <servo name="ELEVON_LEFT"  no="4" min="1000" neutral="1500" max="2000"/>
    <servo name="ELEVON_RIGHT"  no="5" min="1000" neutral="1500" max="2000"/>
  </servos>

  <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>

  <command_laws>
    <call fun="motor_mixing_run(autopilot_motors_on,FALSE,values)"/>
    <set servo="A1" value="motor_mixing.commands[0]"/>
    <set servo="A2"  value="motor_mixing.commands[1]"/>
    <set servo="B1" value="motor_mixing.commands[2]"/>
    <set servo="B2"  value="motor_mixing.commands[3]"/>

    <let var="aileron_feedback_left"  value="-@YAW"/>
    <let var="aileron_feedback_right" value="-@YAW"/>

    <let var="elevator_feedback_left"  value="-@PITCH * (9600 + @THRUST)/9600"/>
    <let var="elevator_feedback_right" value="+@PITCH * (9600 + @THRUST)/9600"/>

    <let var="hover_left"  value="6*$aileron_feedback_left+$elevator_feedback_left"/>
    <let var="hover_right" value="6*$aileron_feedback_right+$elevator_feedback_right"/>

    <set servo="ELEVON_LEFT"   value= "$hover_left" />
    <set servo="ELEVON_RIGHT"  value= "$hover_right"/>
  </command_laws>

  <section name="MIXING" prefix="MOTOR_MIXING_">
    <define name="TRIM_ROLL" value="0"/>
    <define name="TRIM_PITCH" value="0"/>
    <define name="TRIM_YAW" value="0"/>
    <define name="NB_MOTOR" value="4"/>
    <define name="SCALE" value="256"/>
    <define name="PITCH_COEF"  value="{ -256, -256,  256,  256 }"/>
    <define name="ROLL_COEF"   value="{  256, -256,  256, -256 }"/>
    <define name="YAW_COEF"    value="{ -256,  256,  128, -128 }"/>
    <define name="THRUST_COEF" value="{  256,  256,  256,  256 }"/>
  </section>

  <include href="conf/airframes/esden/calib/asp22-019.xml"/>

  <section name="IMU" prefix="IMU_">
    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
    <define name="BODY_TO_IMU_THETA" value="90." unit="deg"/>
    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
  </section>

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

   <define name="AUTOPILOT_KILL_WITHOUT_AHRS" value="TRUE"/>
   <define name="USE_THROTTLE_FOR_MOTOR_ARMING" value="TRUE"/>
 </section>

 <section name="BAT">
    <define name="MIN_BAT_LEVEL" value="10.4" units="V"/>
   <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/>
 </section>

  <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
    <define name="SP_MAX_P" value="10000"/>
    <define name="SP_MAX_Q" value="10000"/>
    <define name="SP_MAX_R" value="10000"/>

    <define name="GAIN_P" value="400"/>
    <define name="GAIN_Q" value="400"/>
    <define name="GAIN_R" value="350"/>
  </section>

  <!--////////////////THIS SECTION CONTAINS THE GAINS FOR DIFFERENT SETPOINTS\\\\\\\\\\\\\\\\\\-->
  <section name ="GAIN_SETS">
    <define name="NUMBER_OF_GAINSETS" value="2"/>
    <define name="SCHEDULING_VARIABLE" value="stab_att_sp_euler.theta"/>
    <define name="SCHEDULING_POINTS" value="{0, 20*M_PI/180}"/>
    <define name="SCHEDULING_VARIABLE_FRAC" value="12"/>

    <define name="PHI_P" value="{0, 1000}"/>
    <define name="PHI_D" value="{0, 1000}"/>
    <define name="PHI_I" value="{0, 1000}"/>
    <define name="PHI_DD" value="{0, 1000}"/>

    <define name="PSI_P" value="{0, 1000}"/>
    <define name="PSI_D" value="{0, 1000}"/>
    <define name="PSI_I" value="{0, 1000}"/>
    <define name="PSI_DD" value="{0, 1000}"/>

    <define name="THETA_P" value="{0, 1000}"/>
    <define name="THETA_D" value="{0, 1000}"/>
    <define name="THETA_I" value="{0, 1000}"/>
    <define name="THETA_DD" value="{0, 1000}"/>
  </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_PSI"     value="45."  unit="deg"/>
    <define name="SP_MAX_R"       value="90." unit="deg/s"/>
    <define name="SP_MAX_P"       value="90." unit="deg/s"/>
    <define name="DEADBAND_R"     value="250"/>
    <define name="DEADBAND_A"     value="250"/>
    <define name="SP_PSI_DELTA_LIMIT" value="90" unit="deg"/>

    <!-- reference -->
    <define name="REF_OMEGA_P"  value="800" unit="deg/s"/>
    <define name="REF_ZETA_P"   value="0.85"/>
    <define name="REF_MAX_P"    value="300." unit="deg/s"/>
    <define name="REF_MAX_PDOT" value="RadOfDeg(7000.)"/>

    <define name="REF_OMEGA_Q"  value="800" unit="deg/s"/>
    <define name="REF_ZETA_Q"   value="0.85"/>
    <define name="REF_MAX_Q"    value="300." unit="deg/s"/>
    <define name="REF_MAX_QDOT" value="RadOfDeg(7000.)"/>

    <define name="REF_OMEGA_R"  value="500" unit="deg/s"/>
    <define name="REF_ZETA_R"   value="0.9"/>
    <define name="REF_MAX_R"    value="90." unit="deg/s"/>
    <define name="REF_MAX_RDOT" value="RadOfDeg(900.)"/>

    <define name="PHI_PGAIN"  value="230"/>
    <define name="PHI_DGAIN"  value="170"/>
    <define name="PHI_IGAIN"  value="30"/>

    <define name="THETA_PGAIN"  value="300"/>
    <define name="THETA_DGAIN"  value="50"/>
    <define name="THETA_IGAIN"  value="40"/>

    <define name="PSI_PGAIN"  value="300"/>
    <define name="PSI_DGAIN"  value="150"/>
    <define name="PSI_IGAIN"  value="0"/>

    <!-- feedforward -->
    <define name="PHI_DDGAIN"   value=" 0"/>
    <define name="THETA_DDGAIN" value=" 140"/>
    <define name="PSI_DDGAIN"   value=" 0"/>
  </section>

  <section name="INS" prefix="INS_">
  </section>

 <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
   <define name="HOVER_KP"    value="200"/>
   <define name="HOVER_KD"    value="175"/>
   <define name="HOVER_KI"    value="72"/>
   <define name="GUIDANCE_V_NOMINAL_HOVER_THROTTLE" value ="0.9"/>
  </section>

  <section name="AHRS" prefix="AHRS_">
    <define name="H_X" value=" 0.47577"/>
    <define name="H_Y" value=" 0.11811"/>
    <define name="H_Z" value=" 0.87161"/>
  </section>

 <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
   <define name="PGAIN" value="100"/>
   <define name="DGAIN" value="100"/>
   <define name="IGAIN" value="0"/>
 </section>

 <section name="SIMULATOR" prefix="NPS_">
   <define name="ACTUATOR_NAMES"  value="{&quot;front_motor&quot;, &quot;back_motor&quot;, &quot;right_motor&quot;, &quot;left_motor&quot;}"/>
   <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
 </section>

<!--//////////////INCLUDE THE GAIN SCHEDULING MODULE\\\\\\\\\\\\\\\\-->
 <modules main_freq="512">
   <load name="gps_ubx_ucenter.xml"/>
   <load name="led_safety_status.xml">
   	 <define name="USE_LED_BODY" value="1"/>
         <define name="SAFETY_WARNING_LED" value="BODY"/>
   </load>
   <load name="gain_scheduling.xml"/>
</modules>

  <firmware name="rotorcraft">
    <target name="ap" board="lisa_m_2.0">
      <subsystem name="radio_control" type="ppm">
      	<configure name="RADIO_CONTROL_PPM_PIN" value="UART1_RX"/>
      </subsystem>
      <configure name="LISA_M_BARO" value="BARO_MS5611_SPI"/>
    </target>

    <target name="nps" board="pc">
      <subsystem name="fdm"           type="jsbsim"/>
      <subsystem name="radio_control" type="ppm"/>
    </target>

    <subsystem name="motor_mixing"/>
    <subsystem name="actuators"     type="pwm">
      <define name="SERVO_HZ" value="400"/>
      <define name="USE_SERVOS_7AND8"/>
    </subsystem>

    <subsystem name="telemetry"     type="transparent"/>
    <subsystem name="imu"           type="aspirin_v2.1"/>
    <!--subsystem name="imu"	    type="aspirin_v1.0"/-->
    <subsystem name="gps"           type="ublox"/>
    <subsystem name="stabilization" type="int_quat"/>
    <subsystem name="ahrs"          type="int_cmpl_quat">
      <configure name="USE_MAGNETOMETER" value="0"/>
      <define name="AHRS_USE_GPS_HEADING" value="1"/>
    </subsystem>
    <subsystem name="ins"/>
    <!--define name="USE_INS_NAV_INIT"/-->
    <!--subsystem name="stabilization"   type="int_euler"/>
    <subsystem name="ahrs"          type="int_cmpl_euler"/-->
  </firmware>
</airframe>
