<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">

<flight_plan alt="250" ground_alt="185" lat0="43.46223" lon0="1.27289" max_dist_from_home="400" name="test laas" qfu="270" security_height="25">
  <waypoints>
    <waypoint alt="250" name="HOME" x="0.0" y="0.0"/>
    <waypoint alt="215.0" name="IAF" x="239.7" y="92.9"/>
    <waypoint alt="215.0" name="AF" x="187.0" y="-2.8"/>
    <waypoint alt="185.0" name="RWAY" x="42.3" y="47.5"/>
    <waypoint alt="260.0" name="WAIT1" x="118.0" y="84.9"/>
    <waypoint alt="240.0" name="WAIT2" x="-34.4" y="143.5"/>
  </waypoints>
  <blocks>
    <block name="init formation">
      <!-- formation_init() is already called -->
      <!-- init formation by adding knew slots in the formation -->
      <!-- three UAVs are expected but it can work with 1 or 2 -->
      <call fun="add_slot(21, 10, 0, 0)"/>
      <call fun="add_slot(22, -20, 20, 10)"/>
      <call fun="add_slot(23, -20, -20, -10)"/>
      <set var="form_mode" value="0"/> <!-- form mode global add_slot(id, east, north, up) -->
      <set var="leader_id" value="21"/> <!-- leader id -->
      <set var="nav_mode" value="NAV_MODE_ROLL"/> <!-- alternate navigation mode should be set in formation flight -->
    </block>
    <block name="cirle 1"> <!-- waiting circle out of formation -->
      <call fun="stop_formation()"/>
      <circle radius="nav_radius" until="FALSE" wp="WAIT1"/>
    </block>
    <block name="cirle 2"> <!-- waiting circle out of formation -->
      <call fun="stop_formation()"/>
      <circle radius="nav_radius" until="FALSE" wp="WAIT2"/>
    </block>
    <block name="formation"> <!-- start formation for a follower, leader has to be ready and in formation -->
      <call fun="start_formation()"/> <!-- inform other AC that formation is started -->
      <call fun="formation_flight()"/> <!-- formation control loop -->
    </block>
    <block name="land"> <!-- basic landing function out of formation -->
      <call fun="stop_formation()"/>
      <exception cond="(GROUND_ALT+10> estimator_z)" deroute="stop"/>
      <circle radius="50" until="225 > estimator_z" wp="IAF"/>
      <go alt="215" wp="AF"/>
      <go from="AF" hmode="route" vmode="glide" wp="RWAY"/>
    </block>
    <block name="stop">
      <attitude roll="0.0" throttle="0.0" until="FALSE" vmode="throttle"/>
    </block>
  </blocks>
</flight_plan>
