<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
<!--
The goal of this flightplan is to try to provide an easy, no-brainer flightplan for your first 100pct autonomous Parrot Drone tryout flights.
It should be useable for UAS enthousiasts just starting in the field of autonomous Parrot Drone flights.
This flightplan should be kept as simple as possible and *not* to be extended with more functionality, if one needs that, plz use another flightplan.

For you first flights make sure you have more dan enough space and do not make decisions that would be unwise, you have a brain, best to put it to good use ;)
Your safe aircraft operation is *your* responsibility
-->

<!-- Default flightplan alt set to 30m. This hight makes it fly over most of trees in this world by default. Security height 15m a tradeof between telemetry loss, airframe damage and avarage small building and hill height -->
<flight_plan alt="230" ground_alt="200" lat0="50.799999" lon0="7.566666" max_dist_from_home="100" name="Parrot Simple" security_height="15">
  <header>
//Set Generic options
    #include "autopilot.h"
//Enable AHRS Health test functions
    #include "subsystems/ahrs/ahrs_aligner.h"
//Enable advanced electrical power functions
    #include "subsystems/electrical.h"
//Enable datalink tests
    #include "subsystems/datalink/datalink.h"
#include "firmwares/rotorcraft/guidance/guidance_v.h"
  </header>

  <waypoints>
    <waypoint name="HOME" x="0.0" y="0.0"/>
    <waypoint name="_CLIMB" x="0.0" y="5.0"/>
    <waypoint name="Landingspot" x="5.0" y="5.0"/>
    <waypoint name="_TDEMERGENCY" x="-5.0" y="-5.0"/>
    <waypoint name="A" x="5" y="-10"/>
    <waypoint name="B" x="40" y="-60"/>
  </waypoints>
  <exceptions>
 <!-- Check if battery voltage is low, then try to go to block Land. This is deemed better than to land at that very spot since that can be above something unknown,
  is less safe and has less chance of AC recovery. It states battery LOW, not battery critical -->
 <!--   <exception cond="electrical.bat_low && !(nav_block == IndexOfBlock('Setting home location')) && !(nav_block == IndexOfBlock('Holding point')) && !(nav_block == IndexOfBlock('Land')) && !(nav_block == IndexOfBlock('Emergency'))" deroute="Land"/>-->
    <exception cond="electrical.bat_critical && !(nav_block == IndexOfBlock('Setting home location')) && !(nav_block == IndexOfBlock('Holding point')) && !(nav_block == IndexOfBlock('Land')) && !(nav_block == IndexOfBlock('Emergency'))" deroute="Emergency"/>
    <!-- No connection for some time in flight, then try to land -->
    <exception cond="autopilot_in_flight() && datalink_time > 55 && !(nav_block == IndexOfBlock('Setting home location')) && !(nav_block == IndexOfBlock('Holding point')) && !(nav_block == IndexOfBlock('Land')) && !(nav_block == IndexOfBlock('Emergency'))" deroute="Land"/>
    <!-- If somehow the AC isVALUEVALUE higher than initial ground position plus 60m, just land -->
 <!--tofix only in flight   <exception cond="GetPosAlt() > (GetAltRef() + 60)" deroute="Land"/> -->
    <!-- No link for 20 seconds and no GPS valid position fix, but the AC can still hover...however no option to give commands, emergency landing -->
  <!--  <exception cond="And(datalink_time > 20, !GpsFixValid()) && !(nav_block == IndexOfBlock('Setting home location')) && !(nav_block == IndexOfBlock('Holding point')) && !(nav_block == IndexOfBlock('Emergency'))" deroute="Emergency"/>-->
    <!-- Check if outside a safety circle from where the drone started the climb, if outside come back to where climb was started, defer from default outside behaviour-->
  <exception cond="(((((WaypointX(WP__CLIMB)) - GetPosX())*((WaypointX(WP__CLIMB)) - GetPosX()))) + ((((WaypointY(WP__CLIMB)) - GetPosY())*((WaypointY(WP__CLIMB)) - GetPosY())))) > (MAX_DIST_FROM_HOME*MAX_DIST_FROM_HOME) && !(nav_block == IndexOfBlock('Setting home location')) && !(nav_block == IndexOfBlock('Holding point')) && !(nav_block == IndexOfBlock('Land')) && !(nav_block == IndexOfBlock('Emergency'))" deroute="Land"/>
  </exceptions>
  <blocks>
    <block name="Setting home location"><!-- TODO: make sure there is an indicator in "ardrone2_simple.xml" GCS layout config screen displaying "Waiting for GPS position..."-->
      <call_once fun="NavKillThrottle()"/>
<!-- if no valid fix or GPS accuracy > 11m or no AHRS , it's' a no-go, just wait TODO: timeout and message?-->
    <!-- <while cond="!GpsFixValid() || gps.pacc > 1100 || stateIsAttitudeValid()"/>-->
    <while cond="!GpsFixValid()"/>
<!-- Additional wait to allow for better GPS data in the last secons before takeoff -->
      <while cond="LessThan(NavBlockTime(), 8)"/>
      <call_once fun="NavSetGroundReferenceHere()"/>
<!-- Set here, while setting theoretically GPS position can still jump, thus set in least needed precision order -->
      <call_once fun="NavSetAltitudeReferenceHere()"/>
      <call_once fun="NavSetWaypointHere(WP__TDEMERGENCY)"/>
      <call_once fun="NavSetWaypointHere(WP_Landingspot)"/>
<!-- ALTERNATIVE To make the naming better visible, move it aside a bit
      <set var="waypoints[WP_Landingspot].x" value="WaypointX(WP_Landingspot) + 1"/>
      <set var="waypoints[WP_Landingspot].y" value="WaypointY(WP_Landingspot) + 5"/>
-->
      <call_once fun="NavSetWaypointHere(WP_HOME)"/>
      <call_once fun="NavSetWaypointHere(WP__CLIMB)"/>
    </block>
<!-- wait for take-off signal, remove this part for 100% autonomous flights, no GCS -->
    <block name="Holding point">
      <call_once fun="NavKillThrottle()"/>
      <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
    </block>
<!-- someone wrote: if the throttle is set to 0 in the RC for some time after entering the takeoff block, it is possible that due to GPS fluctuations the CLIMB point will no longer be located directly above the current position of the aircraft.
 From the start the aircraft tries to move horizontally to come back to the CLIMB point. This causes it to try to take off with some pitch and roll which can cause a flip. -->
    <block key="t" name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
<!-- To make sure that even if aircraft was moved by e.g. walking with it to a different spot, the takeoff and landing position are still on that spot, home is allowed to be somewhere else. -->
      <call_once fun="NavSetAltitudeReferenceHere()"/>
      <call_once fun="NavSetWaypointHere(WP__CLIMB)"/>
      <call_once fun="NavSetWaypointHere(WP_Landingspot)"/>
   <!--    <set var="waypoints[WP_Landingspot].z" value="GetAltRef()"/>--> <!-- Maybe should be actual altitude?-->
      <!-- Right from the start the aircraft tries to achieve the reference heading, which is the heading it had when the battery is connected.
       If the aircraft is later moved to a takeoff spot and the heading is changed it will try to rotate to the original heading as it is taking off which can make some motors spin too fast and cause a flip or other unwanted manovering.
Altough Switching the mode to auto2/NAV from any other mode will reset nav_heading to the current heading this is not enough therefore just befor takeo we set the nav heading to the current heading-->

    <!--  old way  <set var="nav_heading" value="stateGetNedToBodyEulers_i()->psi">--> <!-- make sure the current heading is set just befor takeoff as far as possible, MAGNETO anyone?-->
      <call_once fun="nav_set_heading_current()"/> <!-- make sure the current heading is set just befor takeoff as far as possible -->
      <call_once fun="NavResurrect()"/>
      <set value="0" var="autopilot.kill_throttle"/>
      <call_once fun="NavResurrect()"/><!-- Ressurect again, since sometimes once is not enough and motors will not start spinning -->
      <!-- TODO improve try takeof 5 times than give up an go to Holding point, something stuck in the prop, a finger maybe ;)-->
      <!--   <exception cond="!autopilot_motors_on" deroute="Takeoff"/>-->
      <set value="0" var="autopilot.flight_time"/>
      <!-- If take-off to first point takes to long to reach somehow because of some reason, abort flight -->
      <!-- todo test  various times -->
     <exception cond="block_time > 560" deroute="Land"/>
     <!-- To make sure that takoff is straight up to the height of first waypoint A, so no sideways take-off, since sideways movements starting at low height from takeoff can have nasty effects) -->
<!-- In real life aircraft can be blown sideways with strong wind TODO experiment with results of 2 stages 2 -->
<!-- TODO see if adding this 2nd stage is better -->
   <!--  <attitude pitch="0" roll="0" throttle="0.90" until="stage_time > 2" vmode="throttle"/>-->
<!--Alternative <exception cond="WaypointAlt(WP_A) > stateGetPositionEnu_f()->z" deroute="A_to_B_and_back"/>-->
<!-- See http://lists.gnu.org/archive/html/paparazzi-devel/2014-06/msg00138.html -->
    <!--<call_once fun="GuidanceVSetRef(0,0,0)"/>-->
     <!--<call_once fun="nav_set_heading_current()"/>-->
     <stay vmode="climb" climb="0.7" until="WaypointAlt(WP_A) > stateGetPositionEnu_f()->z" wp="_CLIMB"/>
    </block>
<!-- The From A to B and back, The only one visible block so no GCS choices to toy around with, a non expert would get confused -->
    <block key="m" name="From A to B and back">
      <call_once fun="nav_set_heading_towards_waypoint(WP_A)"/>
      <go wp="A"/>
      <call_once fun="nav_set_heading_towards_waypoint(WP_B)"/>
      <go from="A" hmode="route" wp="B"/>
      <call_once fun="nav_set_heading_towards_waypoint(WP_A)"/>
      <stay wp="B" until="stage_time > 3"/> <!-- Hang around for 3 seconds, any longer and people start get worried if all is OK...-->
      <go from="B" hmode="route" wp="A"/>
      <!-- to make sure the route height to touchdown is high not lower than on the first takeoff-->
   <!-- TODO: discuss this behaviour since it will look strange if drone first will climb to climb point and not decend when one presses land, perception difference-->
      <deroute block="Land"/>
    </block>
<!-- A key combination to initiate a landing -->
    <block name="Land" key="l">
      <go wp="_CLIMB"/>
      <go wp="Landingspot"/>
      <exception cond="NavDetectGround()" deroute="landed"/>
      <exception cond="!nav_is_in_flight()" deroute="landed"/>
      <call_once fun="NavStartDetectGround()"/>
<!-- decend with 2ms while staying at same spot-->
      <stay climb="-2.0" vmode="climb" wp="Landingspot"/>
    </block>
<!-- To be able to react to the panic button. On throttle this works better if your battery is almost empty it will not try to throttle up creating a batt dip, and then a mainboard that would stop working -->
    <block name="Emergency" key="e" strip_button="Emergency" strip_icon="home_emergency.png">
       <exception cond="!nav_is_in_flight()" deroute="landed"/>
   <!-- If landing would takes to long take drastic measures, kill everything -->
     <!-- <exception cond="Block_time > 50" value="1" var="autopilot.kill_throttle"/>-->
      <call_once fun="NavSetWaypointHere(WP__TDEMERGENCY)"/>
  <!--TODO: better test detect landing different surfaces and agles with sonar baro combi of AR-Drone 2 -->
      <call_once fun="NavStartDetectGround()"/>
      <!--TODO: this is a throttle level for ARDrone2, should be nominaltrhrootle devided by 2 or so-->
      <stay wp="_TDEMERGENCY" throttle="0.40" vmode="throttle"/> <!-- 0.40 is for AR.Drone 2.0 Bebop could be a lower value -->
    </block>
    <block name="landed">
      <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
    </block>
  </blocks>
</flight_plan>

