<!DOCTYPE airframe SYSTEM "airframe.dtd">

<airframe name="BOOZ2_F1">

  <firmware name="rotorcraft">
    <target name="ap" board="booz_1.0">
      <!--<define name="KILL_MOTORS"/>-->       <!-- prevent motors from ever starting -->
      <!--<define name="RADIO_KILL_SWITCH" value="RADIO_CONTROL_SWITCH1"/>-->
      <define name="USE_INS_NAV_INIT"/>
    </target>

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


    <subsystem name="telemetry"     type="transparent_usb"/>
    <subsystem name="radio_control" type="ppm"/>
    <subsystem name="gps"           type="ublox"/>
    <subsystem name="actuators"     type="mkk"/>
    <subsystem name="imu"           type="b2_v1.0"/>
    <subsystem name="stabilization"   type="euler"/>
    <subsystem name="ahrs"          type="int_cmpl_quat"/>
    <subsystem name="ins"/>
  </firmware>

  <firmware name="booz2_test_progs">
    <target name="test_downlink"  board="booz_1.0"/>
    <target name="test_baro"      board="booz_1.0"/>
    <target name="test_spektrum"  board="booz_1.0"/>
    <target name="tunnel"         board="booz_1.0"/>
    <target name="usb_tunnel_0"   board="booz_1.0"/>
    <target name="usb_tunnel_1"   board="booz_1.0"/>
  </firmware>

  <modules main_freq="512">
    <load name="booz_pwm.xml">
      <define name="USE_PWM1"/>
    </load>
    <load name="booz_drop.xml"/>
    <load name="sys_mon.xml"/>
    <!--load name="booz_cam.xml"/-->
    <load name="sonar_maxbotix_booz.xml">
      <configure name="ADC_SONAR" value="ADC_0"/>
    </load>
  </modules>

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

  <section name="ACTUATORS_MKK" prefix="ACTUATORS_MKK_">
    <define name="NB" value="4" />
    <define name="ADDR" value="{ 0x52, 0x54, 0x58, 0x56 }" />
  </section>

  <section name="SUPERVISION" prefix="SUPERVISION_">
    <define name="MIN_MOTOR" value="2" />
    <define name="MAX_MOTOR" value="210" />
    <define name="TRIM_A" value="0" />
    <define name="TRIM_E" value="0" />
    <define name="TRIM_R" value="0" />
    <define name="NB_MOTOR" value="4" />
    <define name="SCALE" value="256"/>
    <define name="ROLL_COEF"   value="{   0,    0, -256,  256}" />
    <define name="PITCH_COEF"  value="{ 256, -256,    0,    0}" />
    <define name="YAW_COEF"    value="{ 256,  256, -256, -256}" />
    <define name="THRUST_COEF" value="{ 256,  256,  256,  256}"/>
  </section>

  <section name="IMU" prefix="IMU_">
    <define name="GYRO_P_NEUTRAL" value="32362" />
    <define name="GYRO_Q_NEUTRAL" value="32080" />
    <define name="GYRO_R_NEUTRAL" value="32096" />

    <define name="GYRO_P_SENS" value="1.1032765" integer="16" />
    <define name="GYRO_Q_SENS" value="1.1360802599" integer="16" />
    <define name="GYRO_R_SENS" value="1.1249874614" integer="16" />

    <define name="ACCEL_X_SENS" value="2.45932966" integer="16"/>
    <define name="ACCEL_Y_SENS" value="2.45106376" integer="16"/>
    <define name="ACCEL_Z_SENS" value="2.47825717" integer="16"/>

    <define name="ACCEL_X_NEUTRAL" value="31886"/>
    <define name="ACCEL_Y_NEUTRAL" value="32162"/>
    <define name="ACCEL_Z_NEUTRAL" value="32941"/>

    <define name="MAG_X_CHAN" value="4" />
    <define name="MAG_Y_CHAN" value="0" />
    <define name="MAG_Z_CHAN" value="2" />

    <define name="MAG_45_HACK" value="1" />
    <!--define name="MAG_X_SENS" value="5.14821844457 * sqrt(2)/2" integer="16"/-->
    <!--define name="MAG_Y_SENS" value="5.11810156597 * sqrt(2)/2" integer="16"/-->
    <define name="MAG_X_SENS" value="3.640340173" integer="16"/>
    <define name="MAG_Y_SENS" value="3.619044324" integer="16"/>
    <define name="MAG_Z_SENS" value="5.07618333556" integer="16"/>

    <define name="MAG_X_SIGN" value="-1" />
    <define name="MAG_Y_SIGN" value=" 1" />
    <define name="MAG_Z_SIGN" value="-1" />

    <define name="MAG_X_NEUTRAL" value="2059"/>
    <define name="MAG_Y_NEUTRAL" value="1944"/>
    <define name="MAG_Z_NEUTRAL" value="2099"/>

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

  <section name="AHRS" prefix="AHRS_">
    <define name="PROPAGATE_FREQUENCY" value="512"/>
    <define name="H_X" value=" 0.51562740288882"/>
    <define name="H_Y" value="-0.05707735220832"/>
    <define name="H_Z" value=" 0.85490967783446"/>
  </section>

  <section name="STABILIZATION_RATE" prefix="STABILIZATION_RATE_">
    <!-- setpoints -->
    <define name="SP_MAX_P" value="10000" />
    <define name="SP_MAX_Q" value="10000" />
    <define name="SP_MAX_R" value="10000" />
    <define name="DEADBAND_P" value="0" />
    <define name="DEADBAND_Q" value="0" />
    <define name="DEADBAND_R" value="200" />
    <define name="REF_TAU" value="4" />

    <!-- feedback -->
    <define name="GAIN_P" value="400" />
    <define name="GAIN_Q" value="400" />
    <define name="GAIN_R" value="350" />

    <define name="IGAIN_P" value="75" />
    <define name="IGAIN_Q" value="75" />
    <define name="IGAIN_R" value="50" />

    <!-- feedforward -->
    <define name="DDGAIN_P" value="300" />
    <define name="DDGAIN_Q" value="300" />
    <define name="DDGAIN_R" value="300" />
  </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="90." unit="deg/s" />
    <define name="DEADBAND_R" value="250" />

    <!-- reference -->
    <define name="REF_OMEGA_P" value="800" unit="deg/s" />
    <define name="REF_ZETA_P" value="0.9" />
    <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.9" />
    <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="180." unit="deg/s" />
    <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)" />

    <!-- feedback -->
    <define name="PHI_PGAIN" value="1000" />
    <define name="PHI_DGAIN" value="300" />
    <define name="PHI_IGAIN" value="100" />

    <define name="THETA_PGAIN" value="1000" />
    <define name="THETA_DGAIN" value="300" />
    <define name="THETA_IGAIN" value="100" />

    <define name="PSI_PGAIN" value="500" />
    <define name="PSI_DGAIN" value="320" />
    <define name="PSI_IGAIN" value="75" />

    <!-- feedforward -->
    <define name="PHI_DDGAIN" value=" 300" />
    <define name="THETA_DDGAIN" value=" 300" />
    <define name="PSI_DDGAIN" value=" 300" />

  </section>

  <section name="INS" prefix="INS_">
    <define name="BARO_SENS" value="15." integer="16"/>
    <define name="SONAR_SENS" value="2.146" integer="16"/>
  </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="HOVER_KP" value="100"/>
    <define name="HOVER_KD" value="60"/>
    <define name="HOVER_KI" value="10"/>
    <!-- 1.5m/s for full stick : SPEED_BFP_OF_REAL(1.5) / (MAX_PPRZ/2) -->
    <define name="RC_CLIMB_COEF" value="163" />
    <!-- SPEED_BFP_OF_REAL(1.5) * 20% -->
    <define name="RC_CLIMB_DEAD_BAND" value="160000" />
  </section>


  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
    <define name="PGAIN" value="70"/>
    <define name="DGAIN" value="150"/>
    <define name="IGAIN" value="15"/>
  </section>

  <section name="BAT">
    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V" />
    <define name="MAX_BAT_LEVEL" value="12.0" unit="V" />
    <define name="VoltageOfAdc(adc)" value="(0.0246*adc)"/>
  </section>

  <section name="MISC">
    <define name="FACE_REINJ_1" value="1024"/>
    <define name="BOOZ_ANALOG_BARO_THRESHOLD" value="512"/>
  </section>

  <section name="FMS">
  </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="INITIAL_CONDITITONS" value="&quot;reset00&quot;" />
    <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_booz2_a1.h&quot;" />
  </section>

  <section name="MODE" prefix="MODE_">
    <define name="MANUAL" value="AP_MODE_ATTITUDE_DIRECT" />
    <define name="AUTO1" value="AP_MODE_ATTITUDE_Z_HOLD" />
    <define name="AUTO2" value="AP_MODE_HOVER_Z_HOLD" />
  </section>

</airframe>
