<!-- this is a quadshot vehicle equiped with Lisa/m2.0 and an aspirin 2.1
     More information on the Quadshot can be found at transition-robotics.com -->

<airframe name="quadshot aspirin 2.1 spektrum">

  <servos driver="Pwm">
    <servo name="A1" no="0" min="1000" neutral="1100" max="1600"/>
    <servo name="A2" no="1" min="1000" neutral="1100" max="1600"/>
    <servo name="B1"  no="2" min="1000" neutral="1100" max="1600"/>
    <servo name="B2"  no="3" min="1000" neutral="1100" max="1600"/>
<!--    <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="1150" neutral="1500" max="1850"/>
    <servo name="ELEVON_RIGHT"  no="5" min="1150" neutral="1500" max="1850"/>-->
    <servo name="ELEVON_LEFT"  no="4" min="1300" neutral="1500" max="1700"/>
    <servo name="ELEVON_RIGHT"  no="5" min="1300" neutral="1500" max="1700"/>
  </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_get_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]"/>

 <!-- Mode dependent actuator laws for the elevons. The elevons act different in rc attitude flight mode-->
 <!-- First the correct feedback is stored in variables -->
    <let var="aileron_feedback_left"  value="-@YAW"/>
    <let var="aileron_feedback_right" value="-@YAW"/>

    <let var="elevator_feedback_left"  value="-@PITCH"/>
    <let var="elevator_feedback_right" value="+@PITCH"/>

 <!-- Here the gains are defined for the two feedback cases, hover and forward-->
    <let var="hover_left"  value="10*$aileron_feedback_left"/>
    <let var="hover_right" value="10*$aileron_feedback_right"/>

<!--     if using INDI -->
<!--    <let var="forward_left"  value="aileron_gain*$aileron_feedback_left+elevator_gain*$elevator_feedback_left"/>
    <let var="forward_right" value="aileron_gain*$aileron_feedback_right+elevator_gain*$elevator_feedback_right"/>-->

<!--     if using PID with gain scheduling -->
    <let var="forward_left"  value="3*$aileron_feedback_left + 4*$elevator_feedback_left"/>
    <let var="forward_right" value="3*$aileron_feedback_right + 4*$elevator_feedback_right"/>

    <!-- This statement tells the autopilot to use the hover feedback if in mode attitude direct and to use the forward feedback in all other cases-->
    <set servo="ELEVON_LEFT"    value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_left  : $forward_left" />
    <set servo="ELEVON_RIGHT"   value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? $hover_right  : $forward_right" />
<!--     <set servo="ELEVON_RIGHT"   value="AP_MODE == AP_MODE_ATTITUDE_DIRECT ? 9000 : -9000" /> -->
  </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 }"/>
<!--    <define name="PITCH_COEF"  value="{ 0, 0, 0, 0 }"/>
    <define name="ROLL_COEF"   value="{ 0, 0, 0, 0 }"/>
    <define name="YAW_COEF"    value="{ 0, 0, 0, 0 }"/>
    <define name="THRUST_COEF" value="{ 0, 0, 0, 0 }"/>-->
  </section>

  <!-- If you got a caliberation XML document from your IMU supplier, import this in the IMU section -->

  <!-- Note that is better to have *no* caliberation file at all, than a one with incorrect caliberation values.
       The default factory values are most of the time perfectly acceptable, so by default we will not use the caliberation file -->

  <section name="IMU" prefix="IMU_">
    <!-- Use default driver values for gyro -->

    <!-- IMU calibration, make sure to calibrate the IMU properly before flight, see the wiki for more info-->
    <define name="ACCEL_X_NEUTRAL" value="0"/>
    <define name="ACCEL_Y_NEUTRAL" value="0"/>
    <define name="ACCEL_Z_NEUTRAL" value="0"/>
    <define name="ACCEL_X_SENS" value="4.86487566223" integer="16"/>
    <define name="ACCEL_Y_SENS" value="4.89957269597" integer="16"/>
    <define name="ACCEL_Z_SENS" value="4.82616398266" integer="16"/>

<!--     <define name="MAG_X_NEUTRAL" value="104"/> -->
<!--     <define name="MAG_Y_NEUTRAL" value="9"/> -->
<!--     <define name="MAG_Z_NEUTRAL" value="15"/> -->
<!--     <define name="MAG_X_SENS" value="3.57880347067" integer="16"/> -->
<!--     <define name="MAG_Y_SENS" value="3.54049742843" integer="16"/> -->
<!--     <define name="MAG_Z_SENS" value="4.0620533034" integer="16"/> -->

	<define name="MAG_X_NEUTRAL" value="-86"/>
	<define name="MAG_Y_NEUTRAL" value="-36"/>
	<define name="MAG_Z_NEUTRAL" value="35"/>
	<define name="MAG_X_SENS" value="3.98639595061" integer="16"/>
	<define name="MAG_Y_SENS" value="4.00632407649" integer="16"/>
	<define name="MAG_Z_SENS" value="4.17946082997" integer="16"/>
  </section>

  <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_MANUAL"  value="AP_MODE_ATTITUDE_DIRECT"/>
    <define name="MODE_AUTO1" value="AP_MODE_FORWARD"/>
    <define name="MODE_AUTO2"  value="AP_MODE_NAV"/>

<!--     <define name="USE_YAW_FOR_MOTOR_ARMING" value="TRUE"/> -->
  </section>

  <section name="BAT">
<!--     <define name="MIN_BAT_LEVEL" value="10.8" units="V"/> -->
    <define name="CRITIC_BAT_LEVEL" value="10.8" unit="V"/>
    <define name="LOW_BAT_LEVEL" value="10.8" unit="V"/>
    <define name="CATASTROPHIC_BAT_LEVEL" value="10.1" 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="350"/>
    <define name="GAIN_Q" value="250"/>
    <define name="GAIN_R" value="350"/>

    <define name="IGAIN_P" value="200"/>
    <define name="IGAIN_Q" value="200"/>
    <define name="IGAIN_R" value="200"/>
  </section>

  <!-- These gains are used when the gain scheduling module is enabled (by loading it in the modules section)-->
  <section name ="GAIN_SETS">
    <define name="NUMBER_OF_GAINSETS" value="2"/>
    <define name="SCHEDULING_VARIABLE" value="guidance_hybrid_norm_ref_airspeed"/>
    <define name="SCHEDULING_POINTS" value="{4, 10}"/>
    <define name="SCHEDULING_VARIABLE_FRAC" value="8"/>


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

    <define name="THETA_P" value="{100, 80}"/>
    <define name="THETA_D" value="{100, 110}"/>
    <define name="THETA_I" value="{40, 40}"/>
    <define name="THETA_DD" value="{0, 0}"/>

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

  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">

    <!-- setpoints -->
    <define name="SP_MAX_PHI"     value="60." unit="deg"/>
    <define name="SP_MAX_THETA"   value="60." unit="deg"/>
    <define name="SP_MAX_R"       value="100." 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="1500" 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="1500" 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="1500" unit="deg/s"/>
    <define name="REF_ZETA_R"   value="0.85"/>
    <define name="REF_MAX_R"    value="300." unit="deg/s"/>
    <define name="REF_MAX_RDOT" value="RadOfDeg(7000.)"/>

    <!-- feedback -->
    <define name="PHI_PGAIN"  value="150"/>
    <define name="PHI_DGAIN"  value="150"/>
    <define name="PHI_IGAIN"  value="30"/>

    <define name="THETA_PGAIN"  value="100"/>
    <define name="THETA_DGAIN"  value="100"/>
    <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=" 0"/>
    <define name="PSI_DDGAIN"   value=" 0"/>
  </section>

  <section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
    <!-- setpoints -->
    <define name="SP_MAX_PHI" value="60." unit="deg"/>
    <define name="SP_MAX_THETA" value="60." unit="deg"/>
    <define name="SP_MAX_R" value="180." 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"/>
    <!-- control effectiveness -->
    <define name="G1_P" value="0.0179"/>
    <define name="G1_Q" value="0.0708"/>
    <define name="G1_R" value="0.0095"/>
    <define name="G2_R" value="0.0"/>
    <!-- reference acceleration for attitude control -->
    <define name="REF_ERR_P" value="70.0"/>
    <define name="REF_ERR_Q" value="45.0"/>
    <define name="REF_ERR_R" value="70.0"/>
    <define name="REF_RATE_P" value="9.0"/>
    <define name="REF_RATE_Q" value="16.0"/>
    <define name="REF_RATE_R" value="12.0"/>
    <!-- second order filter parameters -->
    <define name="FILT_CUTOFF" value="8.0"/>
    <define name="FILT_CUTOFF_R" value="3.2"/>
    <!-- first order actuator dynamics -->
    <define name="ACT_DYN_P" value="0.03"/>
    <define name="ACT_DYN_Q" value="0.03"/>
    <define name="ACT_DYN_R" value="0.03"/>
        <!-- Adaptive Learning Rate -->
    <define name="USE_ADAPTIVE" value="TRUE"/>
    <define name="ADAPTIVE_MU" value="0.0001"/>
  </section>


 <!-- Gains for vertical navigation -->
  <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="HOVER_KP"    value="200"/>
    <define name="HOVER_KD"    value="175"/>
    <define name="HOVER_KI"    value="100"/>
    <define name="NOMINAL_HOVER_THROTTLE" value ="0.2"/>
    <define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
  </section>

  <section name="AHRS" prefix="AHRS_">
    <!--     Delft magnetic field -->
    <define name="H_X" value="0.39049610"/>
    <define name="H_Y" value="0.00278894"/>
    <define name="H_Z" value="0.92060036"/>

    <!-- For vibrating airfames -->
    <define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
  </section>

 <!-- Gains for horizontal navigation-->
  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
    <define name="PGAIN" value="50"/>
    <define name="DGAIN" value="50"/>
    <define name="IGAIN" value="65"/>

    <define name="MAX_BANK" value="RadOfDeg(55)"/>
    <define name="FORWARD_MAX_BANK" value="RadOfDeg(35)"/>
    <define name="USE_REF" value="FALSE"/>
  </section>

  <section name="NAV">
    <define name="ARRIVED_AT_WAYPOINT" value="5" unit="m"/>
  </section>

  <section name="MISC">
    <define name="RADIO_CONTROL_SPEKTRUM_SIGNS" value="{1,1,1,1,1,1,1,1,1,1,1,1}"/>
    <!--The Quadshot uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
    <define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
    <!-- This is the pitch angle that the Quadshot will have in forward flight, where 0 degrees is hover-->
    <define name="TRANSITION_MAX_OFFSET" value="-75.0" unit="deg"/>

    <define name="RC_LOST_MODE" value="MODE_AUTO2"/>

    <define name="SWITCH_STICKS_FOR_RATE_CONTROL" value="TRUE"/>

    <define name="AIRSPEED_ETS_I2C_DEV" value="i2c2"/>

    <define name="AHRS_MAG_UPDATE_ALL_AXES" value="FALSE"/>

    <define name="THRESHOLD_GROUND_DETECT" value="25.0"/>
    <define name="KILL_ON_GROUND_DETECT" value="TRUE"/>
    <define name="FAILSAFE_GROUND_DETECT" value="TRUE"/>
    <define name="HYBRID_NAVIGATION" value="TRUE"/>
    <define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
    <define name="ADAPTIVE_INDI" value="FALSE"/>
  </section>

  <modules main_freq="512">
    <load name="gps_ubx_ucenter.xml"/>

    <!-- The the led_safety_status module will make the Quadshot LEDs blink in certain patterns when there are safety violations-->
    <load name="led_safety_status.xml">
          <define name="USE_LED_BODY" value="1"/>
          <define name="SAFETY_WARNING_LED" value="BODY"/>
    </load>

    <load name="sonar_adc.xml">
      <configure name="ADC_SONAR" value="ADC_2"/>
<!--       <define name="USE_SONAR"/> -->
      <define name="SENSOR_SYNC_SEND_SONAR"/>
      <define name="SONAR_SCALE" value="0.0032"/>
    </load>

  <!--Use an airspeed sensor and get the measured airspeed in the messages-->
<!--    <load name="airspeed_ets.xml">
      <define name="AIRSPEED_ETS_SYNC_SEND"/>
    </load>-->

    <module name="logger_spi_link"/>

<!--    <load name="AOA_adc.xml">
      <configure name="ADC_AOA" value="ADC_2"/>
      <define name="AOA_OFFSET" value="0"/>
      <define name="AOA_FILTER" value="0"/>
      <define name="USE_AOA"/>
      <define name="AOA_SENS" value="2.0*M_PI/1024/4"/>
    </load>-->

  <!-- Load this module to use multiple gain sets, which have to be specified in the gain sets section. Does not work with INDI -->
    <!-- <module name="gain_scheduling"/> -->
  </modules>

  <firmware name="rotorcraft">
    <target name="ap" board="lisa_m_2.0">
    <module name="radio_control" type="spektrum">
      <define name="RADIO_KILL_SWITCH" value="5"/>
      <!-- Put the mode on channel AUX1-->
      <define name="RADIO_MODE" value="4"/>
<!--       <configure name="USE_SECONDARY_SPEKTRUM_RECEIVER" value="1"/> -->
    </module>

    <!-- ONLY if flashing via JTAG cable -->
    <configure name="FLASH_MODE"   value="SWD"/>
    <configure name="BMP_PORT" value="/dev/ttyACM0" />

 <!-- To use an airspeed sensor on I2C, enable I2C2-->
<!--      <define name="USE_I2C2"/> -->


      <!-- If using aspirin 2.2, make sure to uncomment the following barometer configuration: -->
      <!--       <configure name="LISA_M_BARO" value="BARO_MS5611_SPI"/> -->
      <configure name="LISA_M_BARO" value="BARO_BOARD_BMP085"/>
    </target>

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

    <module name="telemetry"     type="xbee_api"/>
<!--    <module name="telemetry"     type="transparent"/> -->
    <module name="imu"           type="aspirin_v2.1"/>
    <module name="gps"           type="ublox"/>
<!--     <module name="stabilization" type="int_quat"/> -->
    <module name="stabilization" type="indi_simple"/>
    <module name="guidance"      type="hybrid"/>

    <module name="ahrs"          type="int_cmpl_quat">
      <configure name="USE_MAGNETOMETER" value="1"/>
      <define name="AHRS_USE_GPS_HEADING" value="0"/>
    </module>

    <module name="ins"/>
  </firmware>
</airframe>
