paparazzi/conf/autopilot/rotorcraft_oneloop_switch.xml

<!DOCTYPE autopilot SYSTEM "autopilot.dtd">

<autopilot name="Oneloop Autopilot Rotorcraft">

  <state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">

    <includes>
      <include name="generated/airframe.h"/>
      <include name="autopilot.h"/>
      <include name="autopilot_rc_helpers.h"/>
      <include name="navigation.h"/>
      <include name="guidance.h"/>
      <include name="oneloop/oneloop_andi.h"/>
      <include name="stabilization/stabilization_attitude.h"/>
      <include name="modules/radio_control/radio_control.h"/>
      <include name="modules/gps/gps.h"/>
      <include name="modules/actuators/actuators.h"/>
      <include name="modules/rot_wing_drone/rotwing_state_V2.h"/>
      <define  name="MODE_MANUAL"    value="AP_MODE_ATTITUDE_DIRECT"  cond="ifndef MODE_MANUAL"/>
      <define  name="MODE_ONELOOP"   value="AP_MODE_MODULE"           cond="ifndef MODE_AUTO1"/>
      <define  name="MODE_AUTO2"     value="AP_MODE_NAV"              cond="ifndef MODE_AUTO2"/>
      <define  name="RCLost()"       value="(radio_control.status == RC_REALLY_LOST)"/>
      <define  name="DLModeNav()"    value="(autopilot_mode_auto2 == AP_MODE_NAV)"/>
      <define  name="DLModeGuided()" value="(autopilot_mode_auto2 == AP_MODE_GUIDED)"/>
      <define  name="DLModeModule()" value="(MODE_AUTO1 == AP_MODE_MODULE)"/>
      <define  name="DLModeAZH()"    value="(MODE_AUTO1 == AP_MODE_ATTITUDE_Z_HOLD)"/>
      <define  name="ONELOOP_CONTROLLER" value="TRUE"/>
    </includes>

    <settings>
      <dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
      <dl_setting var="autopilot_mode_auto2" min="2" step="1" max="3" module="autopilot" values="NAV|GUIDED"/>
    </settings>

    <control_block name="set_commands">
      <call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
    </control_block>
    
    <control_block name="run_attitude_control">
      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
      <call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
    </control_block>

    <control_block name="run_guidance_control">
      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
      <call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
      <call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
    </control_block>

    <control_block name="update_v_sp_gcs">
      <call fun="guidance_v_set_z(-nav.nav_altitude)"/>
    </control_block>

    <exceptions>
      <exception cond="nav.too_far_from_home" deroute="HOME"/>
      <exception cond="kill_switch_is_on()" deroute="KILL"/>
    </exceptions>

    <!-- Kill throttle mode 0-->
    <mode name="KILL">
      <select cond="kill_switch_is_on()"/>
      <on_enter>
        <call fun="autopilot_set_in_flight(false)"/>
        <call fun="autopilot_set_motors_on(false)"/>
        <call fun="stabilization.cmd[COMMAND_THRUST] = 0"/>
        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
      </on_enter>
      <control freq="NAVIGATION_FREQUENCY">
        <call fun="nav_periodic_task()"/>
      </control>
      <control>
        <call fun="SetCommands(commands_failsafe)"/>
      </control>
    </mode>

    <!-- Failsafe mode 1-->
    <mode name="FAILSAFE" shortname="FAIL">
    <!-- Failsafe does not work needs to be fixed-->
      <on_enter>
        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
        <call fun="guidance_v_set_z(SPEED_BFP_OF_REAL(FAILSAFE_DESCENT_SPEED))"/>
      </on_enter>
      <control>
        <call fun="gv_update_ref_from_zd_sp(guidance_v.zd_sp, stateGetPositionNed_i()->z)"/>
        <call fun="guidance_v_update_ref()"/>
        <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
        <call_block name="set_commands"/>
      </control>
      <exception cond="!GpsIsLost()" deroute="$LAST_MODE"/>
    </mode>
    
    <!-- Home mode 2-->
    <mode name="HOME">
      <on_enter>
        <call fun="guidance_h_nav_enter()"/>
      </on_enter>
      <control freq="NAVIGATION_FREQUENCY">
        <call fun="nav_home()"/>
      </control>
      <control>
        <call_block name="run_guidance_control"/>
      </control>
      <exception cond="GpsIsLost()" deroute="FAILSAFE"/>
    </mode>
    
    <!-- Rate mode 3-->
    <mode name="RATE_DIRECT" shortname="RATE">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- Attitude mode 4-->
    <mode name="ATTITUDE_DIRECT" shortname="ATT_Quad">
      <select cond="RCMode0()"/>
      <on_enter>
        <call fun="stabilization_attitude_enter()"/>
      </on_enter>
      <control freq="NAVIGATION_FREQUENCY">
        <call fun="nav_periodic_task()"/>
      </control>
      <control>
        <!-- <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_ATTITUDE)"/> -->
        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,STABILIZATION_ATT_SUBMODE_HEADING)"/>
        <call_block name="run_attitude_control"/>
        <call_block name="set_commands"/>
      </control>
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- Rate climb rc mode 5-->
    <mode name="RATE_RC_CLIMB" shortname="RRCC">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- ATTITUDE_RC_CLIMB: 6 -->
    <mode name="ATTITUDE_RC_CLIMB" shortname="ATTITUDE_RC_CLIMB">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- ATTITUDE_CLIMB: 7 -->
    <mode name="ATTITUDE_CLIMB" shortname="ATTITUDE_CLIMB">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- RATE_Z_HOLD: 8 -->
    <mode name="RATE_Z_HOLD" shortname="RATE_Z_HOLD">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- ATTITUDE_Z_HOLD: 9 -->
    <mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- HOVER_DIRECT: 10 -->
    <mode name="HOVER_DIRECT" shortname="HOVER_DIRECT">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- HOVER_CLIMB: 11 -->
    <mode name="HOVER_CLIMB" shortname="HOVER_CLIMB">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- HOVER_Z_HOLD: 12 -->
    <mode name="HOVER_Z_HOLD" shortname="HOVER_Z_HOLD">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- NAV: 13 -->
    <mode name="NAV">
      <select cond="RCMode2() && RCAP2() && DLModeNav()" exception="HOME"/>
      <on_enter>
        <call fun="oneloop_andi_enter(false, CTRL_INDI)"/>
      </on_enter>
      <control freq="NAVIGATION_FREQUENCY">
        <call fun="nav_periodic_task()"/>
      </control>
      <control>
        <!-- <call fun="nav_periodic_task()"/> -->
        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
        <call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
        <call_block name="update_v_sp_gcs"/>
        <call fun="oneloop_from_nav(autopilot_in_flight())"/>
      </control>
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>  

    <!-- RC_DIRECT: 14 -->
    <mode name="RC_DIRECT" shortname="RC_DIRECT">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- CARE_FREE_DIRECT: 15 -->
    <mode name="CARE_FREE_DIRECT" shortname="CARE_FREE_DIRECT">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- FORWARD: 16 -->
    <mode name="FORWARD" shortname="FORWARD">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- MODULE: 17 -->
    <mode name="MODULE" shortname="ONE">
      <select cond="RCMode2() && RCAP0() && DLModeModule()" exception="HOME"/>
      <on_enter>
        <call fun="oneloop_andi_enter(false, CTRL_ANDI)"/>
      </on_enter>
      <control freq="NAVIGATION_FREQUENCY">
        <call fun="nav_periodic_task()"/>
      </control>
      <control>
        <!-- <call fun="nav_periodic_task()"/> -->
        <!-- <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_MODULE)"/>
        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_MODULE)"/> -->
        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
        <call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
        <call_block name="update_v_sp_gcs"/>
        <call fun="oneloop_from_nav(autopilot_in_flight())"/>
      </control>
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- FLIP: 18 -->
    <mode name="FLIP" shortname="FLIP">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>

    <!-- GUIDED: 19 -->
    <mode name="GUIDED">
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>  

    <!-- Attitude mode fixed wing20-->
    <mode name="ATTITUDE_DIRECT_FWD" shortname="ATT_Fwd">
      <select cond="RCMode1()"/>
      <on_enter>
        <call fun="stabilization_attitude_enter()"/>
      </on_enter>
      <control freq="NAVIGATION_FREQUENCY">
        <call fun="nav_periodic_task()"/>
      </control>
      <control>
        <!-- <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_FORWARD)"/> -->
        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE,STABILIZATION_ATT_SUBMODE_FORWARD)"/>
        <call_block name="run_attitude_control"/>
        <call_block name="set_commands"/>
      </control>
      <exception cond="RCLost()" deroute="FAILSAFE"/>
    </mode>
  </state_machine>

</autopilot>