From 5581cd32af81a7f4329eedc48d7e28f277d585d5 Mon Sep 17 00:00:00 2001
From: Freek van Tienen <freek.v.tienen@gmail.com>
Date: Wed, 16 Sep 2015 09:16:19 +0200
Subject: [PATCH] [conf] Dropping mission IMAV

---
 conf/airframes/TUDelft/airframes/mavtec4.xml  |  2 +-
 conf/airframes/TUDelft/airframes/mavtec5.xml  |  5 +++
 conf/airframes/TUDelft/conf.xml               |  6 +--
 .../Tudelft/rotorcraft_survey_mission.xml     | 43 +++++++++++++++----
 4 files changed, 43 insertions(+), 13 deletions(-)

diff --git a/conf/airframes/TUDelft/airframes/mavtec4.xml b/conf/airframes/TUDelft/airframes/mavtec4.xml
index 4eb65aec5ad..1c75659e07e 100644
--- a/conf/airframes/TUDelft/airframes/mavtec4.xml
+++ b/conf/airframes/TUDelft/airframes/mavtec4.xml
@@ -200,7 +200,7 @@
 
   <section name="SIMULATOR" prefix="NPS_">
     <define name="ACTUATOR_NAMES"  value="{&quot;nw_motor&quot;, &quot;ne_motor&quot;, &quot;se_motor&quot;, &quot;sw_motor&quot;}"/>
-    <define name="JSBSIM_MODEL" value="&quot;simple_x_quad_ccw&quot;"/>
+    <define name="JSBSIM_MODEL" value="&quot;simple_x_quad&quot;"/>
     <define name="SENSORS_PARAMS" value="&quot;nps_sensors_params_default.h&quot;"/>
     <!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
     <define name="JS_AXIS_MODE" value="4"/>
diff --git a/conf/airframes/TUDelft/airframes/mavtec5.xml b/conf/airframes/TUDelft/airframes/mavtec5.xml
index 35a6837fc6f..c1ebfb4078c 100644
--- a/conf/airframes/TUDelft/airframes/mavtec5.xml
+++ b/conf/airframes/TUDelft/airframes/mavtec5.xml
@@ -58,6 +58,11 @@
     <axis name="THRUST" failsafe_value="0"/>
   </commands>
 
+  <section name="MISC">
+    <define name="NAV_DESCEND_VSPEED" value="-1.0"/>
+    <define name="NAV_CLIMB_VSPEED" value="1.0"/>
+  </section>
+
   <section name="MIXING" prefix="MOTOR_MIXING_">
     <!-- front left (CW), front right (CCW), back right (CW), back left (CCW) -->
     <define name="TYPE" value="QUAD_X"/>
diff --git a/conf/airframes/TUDelft/conf.xml b/conf/airframes/TUDelft/conf.xml
index e1519acdff4..264f9b70106 100644
--- a/conf/airframes/TUDelft/conf.xml
+++ b/conf/airframes/TUDelft/conf.xml
@@ -60,10 +60,10 @@
    airframe="airframes/TUDelft/airframes/bebop_indi.xml"
    radio="radios/dummy.xml"
    telemetry="telemetry/default_rotorcraft.xml"
-   flight_plan="flight_plans/dummy.xml"
-   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_att_indi.xml"
+   flight_plan="flight_plans/rotorcraft_basic.xml"
+   settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_guidance.xml settings/estimation/ahrs_secondary.xml settings/estimation/ahrs_float_mlkf.xml settings/estimation/ahrs_int_cmpl_quat.xml settings/control/stabilization_att_indi.xml"
    settings_modules="modules/geo_mag.xml modules/air_data.xml"
-   gui_color="#ffff00000000"
+   gui_color="green"
   />
   <aircraft
    name="MAVTec3_CDW"
diff --git a/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml b/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml
index dcd73018b23..c8460c19686 100644
--- a/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml
+++ b/conf/flight_plans/Tudelft/rotorcraft_survey_mission.xml
@@ -8,6 +8,10 @@
 #ifdef DC_AUTOSHOOT_STOP
 #define LINE_START_FUNCTION dc_Survey(dc_distance_interval);
 #define LINE_STOP_FUNCTION {dc_autoshoot = DC_AUTOSHOOT_STOP;}
+#endif
+=======
+#ifndef DropOpen
+#define DropOpen() FALSE
 #endif
   </header>
 
@@ -52,15 +56,6 @@
       <go wp="CRUISE"/>
       <stay wp="FC"/>
     </block>
-    <block name="ReleaseZone1" strip_button="LookRobot-FC" strip_icon="lookdown.png" group="release">
-      <call fun="NavCopyWaypointPositionOnly(WP_CRUISE,WP_RZ1)"/>
-      <go wp="CRUISE"/>
-      <go wp="RZ1"/>
-      <deroute block="Standby"/>
-    </block>
-    <block name="CAM" strip_button="Follow-Vision-Geolocation-Results" strip_icon="lookdown.png" group="hover">
-      <stay wp="CAM"/>
-    </block>
 
     <block name="stay_p1">
       <stay wp="p1"/>
@@ -79,6 +74,36 @@
       <circle radius="nav_radius" wp="CAM"/>
     </block>
 
+<!-- DROPPINGS -->
+    <block name="drop" group="drop">
+       <go wp="DROP"/>
+       <stay climb="nav_descend_vspeed" vmode="climb" wp="DROP" until="LessThan(stateGetPositionEnu_f()->z, 6)"/>
+       <call fun="DropOpen()"/>
+       <stay climb="nav_climb_vspeed" vmode="climb" wp="DROP" until="stateGetPositionEnu_f()->z > 25"/>
+       <deroute block="land"/>
+    </block>
+    <block name="drop_zone1" group="drop">
+       <go wp="RZ1"/>
+       <stay climb="nav_descend_vspeed" vmode="climb" wp="RZ1" until="LessThan(stateGetPositionEnu_f()->z, 6)"/>
+       <call fun="DropOpen()"/>
+       <stay climb="nav_climb_vspeed" vmode="climb" wp="RZ1" until="stateGetPositionEnu_f()->z > 25"/>
+       <deroute block="land"/>
+    </block>
+    <block name="drop_zone2" group="drop">
+       <go wp="RZ2"/>
+       <stay climb="nav_descend_vspeed" vmode="climb" wp="RZ2" until="LessThan(stateGetPositionEnu_f()->z, 6)"/>
+       <call fun="DropOpen()"/>
+       <stay climb="nav_climb_vspeed" vmode="climb" wp="RZ2" until="stateGetPositionEnu_f()->z > 25"/>
+       <deroute block="land"/>
+    </block>
+    <block name="drop_zone3" group="drop">
+       <go wp="RZ3"/>
+       <stay climb="nav_descend_vspeed" vmode="climb" wp="RZ3" until="LessThan(stateGetPositionEnu_f()->z, 6)"/>
+       <call fun="DropOpen()"/>
+       <stay climb="nav_climb_vspeed" vmode="climb" wp="RZ3" until="stateGetPositionEnu_f()->z > 25"/>
+       <deroute block="land"/>
+    </block>
+
 <!-- LAPS -->
     <block name="race" strip_button="Oval p1-p2-p3-p4" strip_icon="oval.png" group="race">
       <go from="p1" hmode="route" wp="p2"/>