<!DOCTYPE airframe SYSTEM "../airframe.dtd">
<!--
Tobias Muench 2012

example airframe for energyadaptive ctl
aircraft:
Twinstar II
hardware:
twog_1.0 + aspirin + ETS baro + ETS speed
-->

<airframe name="Tob_energyadaptive">

  <firmware name="fixedwing">
    <target name="ap" board="twog_1.0">
      <configure name="PERIODIC_FREQUENCY" value="120"/>
      <configure name="AHRS_PROPAGATE_FREQUENCY" value="100"/>
      <configure name="AHRS_CORRECT_FREQUENCY" value="100"/>
      <define    name="AHRS_TRIGGERED_ATTITUDE_LOOP" />

      <define name="USE_I2C0"/>
      <define name="USE_BARO_ETS"/>
    </target>
    <target name="sim" board="pc">
      <define name="INS_BARO_ID" value="BARO_SIM_SENDER_ID"/>
    </target>

    <define name="USE_AIRSPEED"/>
    <define name="AGR_CLIMB"/>
    <define name="STRONG_WIND"/>
    <define name="WIND_INFO"/>
    <define name="WIND_INFO_RET"/>

    <module name="radio_control" type="ppm"/>          <!-- Radio -->
    <module name="gps" type="ublox"/>                  <!-- GPS -->
    <module name="telemetry"     type="transparent"/>  <!-- Communication -->
    <module name="imu"	type="aspirin_i2c_v1.5">      <!-- IMU -->
      <configure name="ASPIRIN_I2C_DEV" value="i2c0"/>
    </module>
    <module name="ahrs"      type="int_cmpl_quat"/>
    <module name="ins"	type="alt_float">
      <define name="USE_BAROMETER"/>
    </module>
    <module name="control" type="energyadaptive"/>
    <module name="navigation"/>
</firmware>

  <modules>
    <module name="airspeed_ets">
      <define name="AIRSPEED_ETS_SYNC_SEND"/>
      <define name="AIRSPEED_ETS_SCALE"   value="1.44"/> <!-- default 1.8-->
     <!-- <define name="AIRSPEED_ETS_OFFSET"  value="50"/> --> <!-- default 0 -->
     <!-- <configure name="AIRSPEED_ETS_I2C_DEV" value="i2c1"/> -->
    </module>
    <module name="baro_ets">
      <define name="BARO_ETS_SCALE" value="40.0"/>
      <define name="BARO_ETS_ALT_SCALE" value="0.3"/>
      <define name="BARO_ETS_SYNC_SEND"/>
    </module>
    <module name="baro_sim"/>
    <module name="air_data"/>
    <module name="digital_cam">
      <define name="DC_SHUTTER_GPIO" value="GPIOB,GPIO22"/><!-- 18:aux 22:camsw-->
    </module>
    <module name="nav" type="survey_polygon"/>
  </modules>

<!-- ################################################## -->
  <servos>    <!-- Actuators -->
    <servo name="MOTOR_LEFT"     no="0" min="1000" neutral="1000" max="2000"/>
    <!-- <servo name="MOTOR_RIGHT"    no="1" min="1000" neutral="1000" max="2000"/> -->
    <servo name="AILERON_LEFT"   no="7" min="900" neutral="1300" max="1700"/>
    <servo name="AILERON_RIGHT"  no="3" min="1140" neutral="1540" max="1940"/>
    <servo name="ELEVATOR"       no="2" min="1700" neutral="1400" max="1000"/>
    <servo name="RUDDER"         no="6" min="1810" neutral="1410" max="1010"/>
    <servo name="HATCH"          no="4" min="1300" neutral="1300" max="800"/>
  </servos>

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

  <rc_commands>  <!-- commands section -->
    <set command="THROTTLE" value="@THROTTLE"/>
    <set command="ROLL"     value="@ROLL"/>
    <set command="PITCH"    value="@PITCH"/>
    <set command="YAW"      value="@YAW"/>
    <set command="HATCH"    value="@GAIN2"/>
  </rc_commands>

  <section name="MIXER">
     <define name="AILERON_DIFF" value="0.66"/>
     <define name="COMBI_SWITCH" value="1.0"/>
  </section>

  <command_laws>
    <set servo="MOTOR_LEFT"  value="@THROTTLE"/>
    <!-- <set servo="MOTOR_RIGHT" value="@THROTTLE"/> -->
    <set servo="RUDDER" value="@YAW + @ROLL*COMBI_SWITCH"/>
    <let var="roll" value="@ROLL"/>
    <set servo="AILERON_LEFT" value="($roll > 0 ? AILERON_DIFF : 1) * $roll"/>
    <set servo="AILERON_RIGHT" value="($roll > 0 ? 1 : AILERON_DIFF) * $roll"/>
    <set servo="ELEVATOR" value="@PITCH"/>
    <set servo="HATCH" value="@HATCH"/>
  </command_laws>

  <section name="AUTO1" prefix="AUTO1_">
    <define name="MAX_ROLL" value="40" unit="deg"/>
    <define name="MAX_PITCH" value="40" unit="deg"/>
  </section>

  <section name="TRIM" prefix="COMMAND_">
    <define name="ROLL_TRIM" value="0"/>
    <define name="PITCH_TRIM" value="0."/> <!--788 -->
  </section>

<!--
######################################################################################
-->

   <section name="INS" prefix="INS_">
    <define name="ROLL_NEUTRAL_DEFAULT" value="0" unit="deg"/>
    <define name="PITCH_NEUTRAL_DEFAULT" value="0" unit="deg"/>
  </section>

  <section name="IMU" prefix="IMU_">
    <define name="MAG_X_NEUTRAL" value="0"/>
    <define name="MAG_Y_NEUTRAL" value="0"/>
    <define name="MAG_Z_NEUTRAL" value="0"/>
    <define name="MAG_X_SENS" value="1." integer="16"/>
    <define name="MAG_Y_SENS" value="1." integer="16"/>
    <define name="MAG_Z_SENS" value="1." integer="16"/>

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

  <section name="BAT">
    <define name="MILLIAMP_AT_FULL_THROTTLE" value="34000" unit="mA"/>
    <define name="CATASTROPHIC_BAT_LEVEL" value="9.3" unit="V"/> <!--10.5-->
    <define name="CRITIC_BAT_LEVEL" value="9.6" unit="V"/><!--11.-->
    <define name="LOW_BAT_LEVEL" value="9.7" unit="V"/><!--11.5-->
    <define name="MAX_BAT_LEVEL" value="12.5" unit="V"/>
  </section>

  <section name="MISC">
    <define name="CLIMB_AIRSPEED" value="12." unit="m/s"/>
    <define name="NOMINAL_AIRSPEED" value="14." unit="m/s"/>
    <define name="GLIDE_AIRSPEED" value="12." unit="m/s"/>
    <define name="RACE_AIRSPEED" value="25." unit="m/s"/>
    <define name="STALL_AIRSPEED" value="8." unit="m/s"/>
    <define name="AIRSPEED_SETPOINT_SLEW" value="0.2" unit="m/s/s"/> <!--default is 1-->

    <define name="CARROT" value="4." unit="s"/>
    <define name="KILL_MODE_DISTANCE" value="(1.5*MAX_DIST_FROM_HOME)"/>
    <define name="CONTROL_FREQUENCY" value="60" unit="Hz"/> <!-- default is 60Hz -->
    <define name="NO_XBEE_API_INIT" value="TRUE"/>
    <define name="TRIGGER_DELAY" value="1."/>
    <define name="DEFAULT_CIRCLE_RADIUS" value="70."/>
  </section>

  <section name="GLS_APPROACH" prefix="APP_">
    <define name="ANGLE" value="10" unit="deg"/>
    <define name="INTERCEPT_AF_SD" value="100" unit="m"/>
    <define name="TARGET_SPEED" value="13" unit="m/s"/>
  </section>

<!-- ########################################################################################### -->
   <section name="VERTICAL CONTROL" prefix="V_CTL_">
    <define name="POWER_CTL_BAT_NOMINAL" value="11.1" unit="volt"/>
    <!-- outer loop proportional gain -->
    <define name="ALTITUDE_PGAIN" value="0.1049"/>
    <!-- outer loop saturation -->
    <define name="ALTITUDE_MAX_CLIMB" value="5."/> <!--default 2-->
    <!-- auto throttle inner loop -->
    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_THROTTLE" value="0.3"/>
    <define name="AUTO_THROTTLE_NOMINAL_CRUISE_PITCH" value="0."/> <!-- default 0 -->
    <define name="AUTO_THROTTLE_CLIMB_THROTTLE_INCREMENT" value="0.1" unit="%/(m/s)"/>
    <define name="AUTO_THROTTLE_PITCH_OF_VZ_PGAIN" value="0.15"/>

    <define name="AIRSPEED_PGAIN" value="0."/>

    <define name="AUTO_THROTTLE_OF_AIRSPEED_PGAIN" value="0.069"/>
    <define name="AUTO_THROTTLE_OF_AIRSPEED_IGAIN" value="0.01"/>

    <define name="AUTO_PITCH_OF_AIRSPEED_PGAIN" value="0.01"/>
    <define name="AUTO_PITCH_OF_AIRSPEED_IGAIN" value="0.003"/>
    <define name="AUTO_PITCH_OF_AIRSPEED_DGAIN" value="0.03"/>

    <define name="ENERGY_TOT_PGAIN" value="0."/>
    <define name="ENERGY_TOT_IGAIN" value="0."/>
    <define name="ENERGY_DIFF_PGAIN" value="0."/>
    <define name="ENERGY_DIFF_IGAIN" value="0."/>

    <define name="AUTO_GROUNDSPEED_SETPOINT" value="6.0" unit="m/s"/>
    <define name="AUTO_GROUNDSPEED_PGAIN" value="0.75"/>
    <define name="AUTO_GROUNDSPEED_IGAIN" value="0.25"/>

    <define name="THROTTLE_SLEW_LIMITER" value="1" unit="s"/>
  </section>

  <section name="HORIZONTAL CONTROL" prefix="H_CTL_">
    <define name="COURSE_PGAIN" value="0.8"/>
    <define name="COURSE_DGAIN" value="0.3"/>

    <define name="ROLL_MAX_SETPOINT" value="50" unit="deg"/>
    <define name="ROLL_ATTITUDE_GAIN" value="7400."/>
    <define name="ROLL_RATE_GAIN" value="200."/>
    <define name="ROLL_IGAIN" value="100."/>
    <define name="ROLL_KFFA" value="0"/>
    <define name="ROLL_KFFD" value="0"/>

    <define name="PITCH_MAX_SETPOINT" value="25" unit="deg"/>
    <define name="PITCH_MIN_SETPOINT" value="-25" unit="deg"/>
    <define name="PITCH_PGAIN" value="7200."/>
    <define name="PITCH_DGAIN" value="6."/>
    <define name="PITCH_IGAIN" value="100."/>
    <define name="PITCH_KFFA" value="0."/>
    <define name="PITCH_KFFD" value="0."/>

    <define name="PITCH_OF_ROLL" value="RadOfDeg(1.0)"/>
    <define name="AILERON_OF_THROTTLE" value="0.0"/>
  </section>

  <section name="AGGRESSIVE" prefix="AGR_">
    <define name="BLEND_START" value="20"/><!-- Altitude Error to Initiate Aggressive Climb CANNOT BE ZERO!!-->
    <define name="BLEND_END" value="10"/><!-- Altitude Error to Blend Aggressive to Regular Climb Modes  CANNOT BE ZERO!!-->
    <define name="CLIMB_THROTTLE" value="0.60"/><!-- Gaz for Aggressive Climb -->
    <define name="CLIMB_PITCH" value="RadOfDeg(12)"/><!-- Pitch for Aggressive Climb -->
    <define name="DESCENT_THROTTLE" value="0.1"/><!-- Gaz for Aggressive Decent -->
    <define name="DESCENT_PITCH" value="RadOfDeg(-20)"/><!-- Pitch for Aggressive Decent -->
    <define name="CLIMB_NAV_RATIO" value="0.8"/><!-- Percent Navigation for Altitude Error Equal to Start Altitude -->
    <define name="DESCENT_NAV_RATIO" value="1.0"/> <!-- -->
  </section>

  <section name="NAV">
    <define name="NAV_PITCH" value="0."/>
    <define name="NAV_GLIDE_PITCH_TRIM" value="0"/>
  </section>

  <section name="DIGITAL_CAMERA" prefix="DC_">
    <define name="AUTOSHOOT_PERIOD" value="3.0" unit="sec"/>
    <define name="AUTOSHOOT_DISTANCE_INTERVAL" value="50" unit="meter"/>
  </section>

  <section name="FAILSAFE" prefix="FAILSAFE_">
    <define name="DELAY_WITHOUT_GPS" value="3" unit="s"/>
    <define name="DEFAULT_THROTTLE" value="0.3" unit="%"/>
    <define name="DEFAULT_ROLL" value="15" unit="deg"/>
    <define name="DEFAULT_PITCH" value="0" unit="deg"/>
    <define name="HOME_RADIUS" value="DEFAULT_CIRCLE_RADIUS" unit="m"/>
  </section>

  <section name="SIMU">
    <define name="WEIGHT" value ="1."/>
    <define name="YAW_RESPONSE_FACTOR" value =".9"/>   <!--default 1.-->
    <define name="PITCH_RESPONSE_FACTOR" value ="1."/> <!--default 1.-->
    <define name="ROLL_RESPONSE_FACTOR" value ="15."/> <!--default 15-->
  </section>
</airframe>
