<!DOCTYPE autopilot SYSTEM "autopilot.dtd">

<autopilot name="Quadrotor Autopilot (Basic version)">

  <state_machine name="main" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true">

    <control_block name="set_actuators">
      <call fun="SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on)"/>
      <call fun="SetActuatorsFromCommands(commands, autopilot_mode)"/>
    </control_block>

    <control_block name="attitude_loop">
      <call fun="SetAttitudeFromRC(rc_values)"/>
      <call fun="AddAttitudeFromFMS()" cond="fms.enabled"/>
      <call fun="stabilization_attitude_run(autopilot_in_flight)"/>
    </control_block>

    <control_block name="altitude_loop">
      <call fun="SetAltitudeFromFMS()" cond="fms.enabled"/>
      <call fun="gv_update_ref_from_z_sp(guidance_v_z_sp)"/>
      <call fun="run_hover_loop(autopilot_in_flight)"/>
      <call fun="SaturateThrottle(rc_values)"/>
    </control_block>

    <exceptions>
      <exception cond="too_far_from_home" deroute="HOME"/>
    </exceptions>

    <mode name="ATTITUDE" start="stabilization_attitude_enter()">
      <select cond="$DEFAULT_MODE"/>
      <select cond="RCMode0()"/>
      <control freq="512">
        <call_block name="attitude_loop"/>
        <call fun="SetThrottleFromRC(rc_values)"/>
        <call_block name="set_actuators"/>
      </control>
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <mode name="VERTICAL" start="stabilization_attitude_enter()|guidance_v_enter()">
      <select cond="RCMode1()"/>
      <control freq="512">
        <call_block name="attitude_loop"/>
        <call_block name="altitude_loop"/>
        <call_block name="set_actuators"/>
      </control>
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <mode name="NAV" start="guidance_h_nav_enter()|guidance_v_enter()">
      <select cond="RCMode2()" exception="HOME"/>
      <select cond="RCMode2() && DLModeNav()"/>
      <control freq="32">
        <call fun="nav_periodic_task()"/>
      </control>
      <control freq="512">
        <call fun="SetCommandFromAP()"/>
        <call fun="GuidanceNavHorizontal()"/>
        <call fun="GuidanceNavVertical()"/>
        <call fun="AddAttitudeFromRC(rc_values)" cond="!RCLost()"/>
        <call fun="stabilization_attitude_run(autopilot_in_flight)"/>
        <call fun="SaturateThrottle(rc_values)" cond="!RCLost()"/>
        <call_block name="set_actuators"/>
      </control>
      <exception cond="GPSLost()" deroute="FAILSAFE"/>
    </mode>

    <mode name="HOME" start="guidance_h_nav_enter()|guidance_v_enter()">
      <control freq="32">
        <call fun="nav_home()"/>
      </control>
      <control freq="512">
        <call fun="SetCommandFromAP()"/>
        <call fun="GuidanceNavHorizontal()"/>
        <call fun="GuidanceNavVertical()"/>
        <call fun="stabilization_attitude_run(autopilot_in_flight)"/>
        <call_block name="set_actuators"/>
      </control>
      <exception cond="GPSLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- Safe landing -->
    <mode name="FAILSAFE" start="stabilization_attitude_set_failsafe_setpoint()|guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)|guidance_v_zd_sp = SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED)">
      <control freq="512">
        <call fun="SetFailsafeCommand()"/>
        <call fun="stabilization_attitude_run(autopilot_in_flight)"/>
        <call fun="gv_update_ref_from_zd_sp(guidance_v_zd_sp)"/>
        <call fun="run_hover_loop(autopilot_in_flight)"/>
        <call fun="actuators_set(autopilot_motors_on)"/>
      </control>
      <exception cond="!GPSLost()" deroute="$LAST_MODE"/>
    </mode>

  </state_machine>

</autopilot>
