<!DOCTYPE flight_plan SYSTEM "flight_plan.dtd">

<flight_plan alt="40" ground_alt="0" lat0="52.170867" lon0="4.412194" max_dist_from_home="1000" name="Transitioning test" security_height="2">
<!-- <flight_plan alt="40" ground_alt="0" lat0="51.979438" lon0="4.390148" max_dist_from_home="1000" name="Transitioning test" security_height="2"> -->
  <header>
    #include "autopilot.h"
    #include "subsystems/radio_control.h"
    #include "subsystems/electrical.h"
    #include "subsystems/actuators.h"
    #include "firmwares/rotorcraft/guidance/guidance_h.h"
</header>
  <waypoints>
    <waypoint name="HOME" x="0.0" y="0.0"/>
    <waypoint name="STDBY" x="0.0" y="0.0"/>
    <waypoint name="REMOTE_LANDING" x="-64.4" y="31.4"/>
  </waypoints>
  <blocks>
    <block name="Wait GPS">
      <call_once fun="NavKillThrottle()"/>
      <while cond="!GpsFixValid()"/>
    </block>
    <block name="Geo init">
      <while cond="LessThan(NavBlockTime(), 10)"/>
      <call_once fun="NavSetGroundReferenceHere()"/>
    </block>
    <block name="Holding point">
      <call_once fun="NavKillThrottle()"/>
      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
    </block>
    <block name="Start Engine">
      <call_once fun="NavResurrect()"/>
    </block>
    <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
      <exception cond="stateGetPositionEnu_f()->z > 4.0" deroute="Climb"/>
      <call_once fun="NavSetWaypointHere(WP_STDBY)"/>
      <attitude pitch="0" roll="0" throttle="0.4" until="FALSE" vmode="throttle"/>
    </block>
    <block name="Climb">
      <exception cond="stateGetPositionEnu_f()->z > 20.0" deroute="Standby"/>
      <call_once fun="NavSetWaypointHere(WP_STDBY)"/>
      <stay climb="2" vmode="climb" wp="STDBY"/>
    </block>
    <block name="Standby" strip_button="Standby" strip_icon="home.png">
      <exception cond="radio_control.status > RC_OK" deroute="land"/>
      <stay wp="STDBY" until="stage_time > 5"/>
    </block>
    <block name="Fly remote location">
      <exception cond="radio_control.status > RC_OK" deroute="land"/>
      <go wp="REMOTE_LANDING"/>
    </block>
    <block name="Remote landing">
      <exception cond="8.0 > stateGetPositionEnu_f()->z" deroute="descent_slow remote"/>
      <stay climb="-1" vmode="climb" wp="REMOTE_LANDING"/>
    </block>
    <block name="descent_slow remote">
      <exception cond="2.0 > stateGetPositionEnu_f()->z" deroute="flare remote"/>
      <stay climb="-0.5" vmode="climb" wp="REMOTE_LANDING"/>
    </block>
    <block name="flare remote">
      <exception cond="0.3 > sonar_adc.distance" deroute="landed remote"/>
      <attitude pitch="0" roll="0" throttle="0.13" vmode="throttle"/>
    </block>
    <block name="landed remote">
      <call_once fun="NavKillThrottle()"/>
      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
    </block>
    <block name="Start Engine2">
      <call_once fun="NavSetWaypointHere(WP_REMOTE_LANDING)"/>
      <call_once fun="NavResurrect()"/>
    </block>
    <block name="Takeoff2" strip_button="Takeoff" strip_icon="takeoff.png">
      <exception cond="stateGetPositionEnu_f()->z > 4.0" deroute="Climb2"/>
      <attitude pitch="0" roll="0" throttle="0.4" until="FALSE" vmode="throttle"/>
    </block>
    <block name="Climb2">
      <exception cond="stateGetPositionEnu_f()->z > 20.0" deroute="Remote hover"/>
      <call_once fun="NavSetWaypointHere(WP_REMOTE_LANDING)"/>
      <stay climb="2" vmode="climb" wp="REMOTE_LANDING"/>
    </block>
    <block name="Remote hover" strip_button="Standby" strip_icon="home.png">
      <exception cond="radio_control.status > RC_OK" deroute="land"/>
      <stay wp="REMOTE_LANDING" until="stage_time > 5"/>
    </block>
    <block name="land">
      <go wp="STDBY"/>
    </block>
    <block name="descent">
      <exception cond="8.0 > stateGetPositionEnu_f()->z" deroute="descent_slow"/>
      <stay climb="-1" vmode="climb" wp="STDBY"/>
    </block>
    <block name="descent_slow">
      <exception cond="2.0 > stateGetPositionEnu_f()->z" deroute="flare"/>
      <stay climb="-0.5" vmode="climb" wp="STDBY"/>
    </block>
    <block name="flare">
      <exception cond="0.3 > sonar_adc.distance" deroute="landed"/>
      <attitude pitch="0" roll="0" throttle="0.13" vmode="throttle"/>
    </block>
    <block name="landed">
      <call_once fun="NavKillThrottle()"/>
      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
    </block>
  </blocks>
</flight_plan>
