From 1f1dadd6ec9c7010f4ff395264126601089831b3 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 17 Sep 2015 15:52:21 +0200
Subject: [PATCH] [IMAV] Post-Competition Fix

---
 .../Tudelft/rotorcraft_survey_aubel.xml       |  3 ++-
 .../Tudelft/rotorcraft_survey_competition.xml |  3 ++-
 .../Tudelft/rotorcraft_survey_mission.xml     | 20 ++++++++++++++++---
 3 files changed, 21 insertions(+), 5 deletions(-)

diff --git a/conf/flight_plans/Tudelft/rotorcraft_survey_aubel.xml b/conf/flight_plans/Tudelft/rotorcraft_survey_aubel.xml
index 7be834e4744..26376ad1fed 100644
--- a/conf/flight_plans/Tudelft/rotorcraft_survey_aubel.xml
+++ b/conf/flight_plans/Tudelft/rotorcraft_survey_aubel.xml
@@ -24,7 +24,8 @@
     <waypoint name="p4" x="12.6" y="-27.8"/>
     <waypoint name="CAM" x="-31.6" y="-44.0"/>
     <waypoint name="CRUISE" x="-31.6" y="-39.9"/>
-    <waypoint name="TD" x="32.4" y="7.0"/>
+    <waypoint height="15" name="TD" x="32.4" y="7.0"/>
+    <waypoint height="15" x="0" y="0" name="ETD"/>
     <waypoint name="_red1" x="94.0" y="133.0"/>
     <waypoint name="_red2" x="171.4" y="-26.2"/>
     <waypoint name="_red3" x="-32.5" y="-127.4"/>
diff --git a/conf/flight_plans/Tudelft/rotorcraft_survey_competition.xml b/conf/flight_plans/Tudelft/rotorcraft_survey_competition.xml
index c7bd769224f..84b4d02c15c 100644
--- a/conf/flight_plans/Tudelft/rotorcraft_survey_competition.xml
+++ b/conf/flight_plans/Tudelft/rotorcraft_survey_competition.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
 
-<flight_plan alt="33" ground_alt="0" lat0="50.788595" lon0="6.04692" max_dist_from_home="500" name="IMAV2015_competition_outdoor" security_height="2">
+<flight_plan alt="290" ground_alt="260" lat0="50.788595" lon0="6.04692" max_dist_from_home="500" name="IMAV2015_competition_outdoor" security_height="2">
   <waypoints>
     <waypoint lat="50.7888090239206" lon="6.04674224830944" name="_PLATE1"/>
     <waypoint lat="50.7890460391004" lon="6.04647268082583" name="_PLATE2"/>
@@ -28,6 +28,7 @@
     <waypoint lat="50.791818" lon="6.050201" name="p4"/>
     <waypoint height="25" name="CRUISE" x="19.2" y="-55.4"/>
     <waypoint height="15" lat="50.7887171157922" lon="6.04695109268497" name="TD"/>
+    <waypoint height="15" x="0" y="0" name="ETD"/>
     <waypoint lat="50.790144" lon="6.046879" name="_green1A"/>
     <waypoint lat="50.789748" lon="6.047844" name="_green1B"/>
     <waypoint lat="50.789058" lon="6.047072" name="_green1C"/>
diff --git a/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml b/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml
index c1d397dceb9..89e63ddf402 100644
--- a/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml
+++ b/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml
@@ -27,17 +27,17 @@
   <exceptions>
       <exception cond="electrical.bat_low && (exception_flag[0] == 0)" deroute="ComeBackAndLand" exec="set_exception_flag(0)"/>
       <exception cond="electrical.bat_critical && (exception_flag[1] == 0)" deroute="land_here" exec="set_exception_flag(1)"/>
-    <exception cond="(!InsideRED(GetPosX(), GetPosY()) && !(nav_block == IndexOfBlock('flare')) && !(nav_block == IndexOfBlock('landed')))" deroute="land_here"/>
+    <exception cond="(!InsideRED(GetPosX(), GetPosY()) && !(nav_block == IndexOfBlock('flare')) && !(nav_block == IndexOfBlock('landed')) && !(nav_block == IndexOfBlock('WaitGPS')) && !(nav_block == IndexOfBlock('GeoInit')))" deroute="land_emergency"/>
   </exceptions>
 
   <blocks>
 
 <!-- BOOT -->
-    <block name="Wait GPS">
+    <block name="WaitGPS">
       <call fun="NavKillThrottle()"/>
       <while cond="!GpsFixValid()"/>
     </block>
-    <block name="Geo init">
+    <block name="GeoInit">
       <while cond="LessThan(NavBlockTime(), 10)"/>
       <call fun="NavSetGroundReferenceHere()"/>
     </block>
@@ -239,6 +239,20 @@
     <block name="landed" strip_button="Kill" strip_icon="kill.png" group="landing">
       <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
     </block>
+    <block name="land_emergency">
+      <call fun="NavSetWaypointHere(WP_ETD)"/>
+      <deroute block="flare_emergency"/>
+    </block>
+    <block name="flare_emergency">
+      <exception cond="NavDetectGround()" deroute="landed_emergency"/>
+      <exception cond="!nav_is_in_flight()" deroute="landed_emergency"/>
+      <call fun="NavStartDetectGround()"/>
+      <stay climb="nav_descend_vspeed" vmode="climb" wp="ETD"/>
+      <deroute block="landed"/>
+    </block>
+    <block name="landed_emergency">
+      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
   </blocks>
 
 </procedure>