diff --git a/conf/airframes/tudelft/rotwing5.xml b/conf/airframes/tudelft/rotwing5.xml
index ea1d23db85..06fce5ec16 100644
--- a/conf/airframes/tudelft/rotwing5.xml
+++ b/conf/airframes/tudelft/rotwing5.xml
@@ -158,9 +158,7 @@
         <module name="sys_id_auto_doublets"/>
         <module name="ground_detect"/>
         <module name="rotwing_state"/>
-        <module name="preflight_checks">
-            <define name="SDLOG_PREFLIGHT_ERROR" value="TRUE"/>
-        </module>
+        <module name="preflight_checks"/>
         <module name="agl_dist"/>
         <module name="approach_moving_target"/>
 
diff --git a/conf/airframes/tudelft/rotwing6.xml b/conf/airframes/tudelft/rotwing6.xml
index aac4b6abb7..3469af594f 100644
--- a/conf/airframes/tudelft/rotwing6.xml
+++ b/conf/airframes/tudelft/rotwing6.xml
@@ -158,9 +158,7 @@
         <module name="sys_id_auto_doublets"/>
         <module name="ground_detect"/>
         <module name="rotwing_state"/>
-        <module name="preflight_checks">
-            <define name="SDLOG_PREFLIGHT_ERROR" value="TRUE"/>
-        </module>
+        <module name="preflight_checks"/>
         <module name="agl_dist"/>
         <module name="approach_moving_target"/>
 
diff --git a/conf/airframes/tudelft/rotwing_25kg_common.xml b/conf/airframes/tudelft/rotwing_25kg_common.xml
index 3415ab283e..b2be3088d7 100644
--- a/conf/airframes/tudelft/rotwing_25kg_common.xml
+++ b/conf/airframes/tudelft/rotwing_25kg_common.xml
@@ -2,7 +2,7 @@
 
 <airframe>
 
-    <section name="CTRL_EFF_SHED" prefix="ROTWING_EFF_SCHED_">
+    <section name="CTRL_EFF_SCHED" prefix="ROTWING_EFF_SCHED_">
         <define name="IXX_BODY"              value="0.3953"/>
         <define name="IYY_BODY"              value="8.472"/>
         <define name="IZZ"                   value="10.18"/>
@@ -58,6 +58,9 @@
 
         <!-- Air data -->
         <define name="AIR_DATA_CALC_AMSL_BARO"          value="TRUE"/>   
+
+        <!-- SD logger -->
+        <define name="SDLOG_PREFLIGHT_ERROR" value="TRUE"/>
     </section>
 
     <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
diff --git a/conf/airframes/tudelft/rotwing_7kg_common.xml b/conf/airframes/tudelft/rotwing_7kg_common.xml
index 732152f307..712661f4a9 100644
--- a/conf/airframes/tudelft/rotwing_7kg_common.xml
+++ b/conf/airframes/tudelft/rotwing_7kg_common.xml
@@ -1,7 +1,6 @@
 <!DOCTYPE airframe SYSTEM "../airframe.dtd">
 
 <airframe>
-
     <section name="AP_FAILSAFE">
         <!-- <define name="NO_GPS_LOST_WITH_DATALINK_TIME"       value="20"/> --> 
         <define name="NO_GPS_LOST_WITH_RC_VALID"            value="FALSE"/> 
@@ -56,7 +55,8 @@
         <!-- Ground detect -->
         <define name="USE_GROUND_DETECT_INDI_THRUST"        value="TRUE"/>
         <define name="USE_GROUND_DETECT_AGL_DIST"           value="TRUE"/>
-        <define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE"          value="0.24"/>
+        <define name="GROUND_DETECT_SPECIFIC_THRUST_THRESHOLD"  value="-12.0"/>
+        <define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE"       value="0.24"/>
 
         <!-- Flight plan defines -->
         <define name="FLARE_HEIGHT"                         value="12"/>
@@ -72,7 +72,7 @@
 
     <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
         <!-- Limits -->
-        <define name="SP_MAX_PHI"           value="45." unit="deg" />
+        <define name="SP_MAX_PHI"           value="45." unit="deg"/>
         <define name="SP_MAX_THETA"         value="45." unit="deg"/>
         <define name="SP_MAX_R"             value="90." unit="deg/s"/>
         <define name="DEADBAND_R"           value="200"/>
@@ -116,13 +116,13 @@
         <!-- G1 and G2 7 kg-->
         <define name="G1_ROLL"      value="{   0.0,  -15.0,    0.0,   15.0, 0.0, 0.0, 0.0, 0.0,  0.0}"/>
         <define name="G1_PITCH"     value="{   1.5,    0.0,   -1.5,    0.0, 0.0, 0.0, 0.0, 0.0,  0.0}"/>
-        <define name="G1_YAW"       value="{-  0.3,    0.3,   -0.3,    0.3, 0.0, 0.0, 0.0, 0.0,  0.0}"/>
+        <define name="G1_YAW"       value="{  -0.3,    0.3,   -0.3,    0.3, 0.0, 0.0, 0.0, 0.0,  0.0}"/>
         <define name="G1_THRUST"    value="{-0.575, -0.575, -0.575, -0.575, 0.0, 0.0, 0.0, 0.0,  0.0}"/>
         <define name="G1_THRUST_X"  value="{   0.0,    0.0,    0.0,    0.0, 0.0, 0.0, 0.0, 0.0, 0.55}"/>
         <define name="G2"           value="{   0.0,    0.0,    0.0,    0.0, 0.0, 0.0, 0.0, 0.0,  0.0}"/>
 
         <!-- Actuator dynamics -->
-        <define name="ACT_FREQ"             value="{20.0, 20.0, 20.0, 20.0, 52.7, 52.7, 52.7, 52.7, 30.0}"/>
+        <define name="ACT_FREQ"             value="{22.0, 22.0, 22.0, 22.0, 52.7, 52.7, 52.7, 52.7, 30.0}"/>
         <define name="ACT_IS_SERVO"         value="{   0,    0,    0,    0,    1,    1,    1,    1,    0}"/>
         <define name="ACT_IS_THRUSTER_X"    value="{   0,    0,    0,    0,    0,    0,    0,    0,    1}"/>
 
@@ -162,7 +162,7 @@
         <define name="HOVER_KP"                 value="310"/>
         <define name="HOVER_KD"                 value="130"/>
         <define name="HOVER_KI"                 value="10"/>
-        <define name="NOMINAL_HOVER_THROTTLE"   value="0.42"/>
+        <define name="NOMINAL_HOVER_THROTTLE"   value="0.60"/> <!-- 0.6 (~5800 PPRZ) for V3D+ drones -->
         <define name="ADAPT_THROTTLE_ENABLED"   value="FALSE"/>
 
         <!-- Reference -->
@@ -192,7 +192,7 @@
         <define name="QUAD_MAX_DECELERATION"    value="0.75"/>  <!-- Maximum horizontal deceleration in quad mode -->
         <define name="SKEW_UP_AIRSPEED"         value="10.0"/>  <!-- Airspeed where the skewing starts when going up in airspeed -->
         <define name="SKEW_DOWN_AIRSPEED"       value="8.0"/>   <!-- Airspeed where the skewing starts when going down in airspeed -->
-        <define name="STATE_MIN_FW_DIST"        value="150"/>   <!-- Minimum distance to switch to fixed wing mode -->
+        <define name="STATE_MIN_FW_DIST"        value="100"/>   <!-- Switch to quad once the drone is within this radius of Standby -->
 
         <define name="SKEW_REF_MODEL"           value="TRUE"/>  <!-- Enable second order reference model for the skewing command -->
         <define name="SKEW_REF_MODEL_P_GAIN"    value="0.001"/> <!-- Skewing reference model proportional gain -->
diff --git a/conf/airframes/tudelft/rotwing_v3b.xml b/conf/airframes/tudelft/rotwing_v3b.xml
index 647d1a14ae..1c819c5c12 100644
--- a/conf/airframes/tudelft/rotwing_v3b.xml
+++ b/conf/airframes/tudelft/rotwing_v3b.xml
@@ -106,7 +106,6 @@
         <module name="imu" type="cube"/>
         <module name="ins" type="ekf2"/>
 
-        <module name="parachute"/>
         <module name="ekf_aw"/>
 
         <!-- Actuators on dual CAN bus -->
@@ -159,7 +158,7 @@
         <servo no="1"  name="MOTOR_RIGHT"     min="0"     neutral="1000" max="7372"/>
         <servo no="2"  name="MOTOR_BACK"      min="0"     neutral="1000" max="7372"/>
         <servo no="3"  name="MOTOR_LEFT"      min="0"     neutral="1000" max="7372"/>
-        <servo no="4"  name="MOTOR_PUSH"      min="0"     neutral="0"    max="7372"/>
+        <servo no="4"  name="MOTOR_PUSH"      min="0"     neutral="1000"    max="7372"/>
         <servo no="5"  name="SERVO_ELEVATOR"  min="5000"  neutral="5000" max="-5500"/>
         <servo no="6"  name="SERVO_RUDDER"   min="-8191" neutral="0"    max="8191"/>
     </servos>
@@ -262,7 +261,7 @@
         <define name="PFC_ACTUATORS" type="array">
             <!-- Aerodynamic -->
             <field type="struct">
-                <field name="feedback_id" value="SERVO_SERVO_ELEVATOR_IDX"/>
+                <field name="feedback_id" value="255"/>
                 <field name="feedback_id2" value="255"/>
                 <field name="low" value="-4500"/>
                 <field name="high" value="4500"/>
@@ -271,7 +270,7 @@
                 <field name="timeout" value="1"/>
             </field>
             <field type="struct">
-                <field name="feedback_id" value="SERVO_SERVO_RUDDER_IDX"/>
+                <field name="feedback_id" value="255"/>
                 <field name="feedback_id2" value="255"/>
                 <field name="low" value="-4500"/>
                 <field name="high" value="4500"/>
@@ -280,7 +279,7 @@
                 <field name="timeout" value="1"/>
             </field>
             <field type="struct">
-                <field name="feedback_id" value="SERVO_AIL_LEFT_IDX"/>
+                <field name="feedback_id" value="255"/>
                 <field name="feedback_id2" value="255"/>
                 <field name="low" value="-4500"/>
                 <field name="high" value="4500"/>
@@ -289,7 +288,7 @@
                 <field name="timeout" value="1"/>
             </field>
             <field type="struct">
-                <field name="feedback_id" value="SERVO_FLAP_LEFT_IDX"/>
+                <field name="feedback_id" value="255"/>
                 <field name="feedback_id2" value="255"/>
                 <field name="low" value="-4500"/>
                 <field name="high" value="4500"/>
@@ -298,7 +297,7 @@
                 <field name="timeout" value="1"/>
             </field>
             <field type="struct">
-                <field name="feedback_id" value="SERVO_FLAP_RIGHT_IDX"/>
+                <field name="feedback_id" value="255"/>
                 <field name="feedback_id2" value="255"/>
                 <field name="low" value="-4500"/>
                 <field name="high" value="4500"/>
@@ -307,7 +306,7 @@
                 <field name="timeout" value="1"/>
             </field>
             <field type="struct">
-                <field name="feedback_id" value="SERVO_AIL_RIGHT_IDX"/>
+                <field name="feedback_id" value="255"/>
                 <field name="feedback_id2" value="255"/>
                 <field name="low" value="-4500"/>
                 <field name="high" value="4500"/>
@@ -318,7 +317,7 @@
 
             <!-- Rotation -->
             <field type="struct">
-                <field name="feedback_id" value="SERVO_ROTATION_MECH_IDX"/>
+                <field name="feedback_id" value="255"/>
                 <field name="feedback_id2" value="255"/>
                 <field name="low" value="-9600"/>
                 <field name="high" value="9600"/>
@@ -370,7 +369,7 @@
                 <field name="low" value="-9600"/>
                 <field name="high" value="2000"/>
                 <field name="low_feedback" value="0"/>
-                <field name="high_feedback" value="2800"/>
+                <field name="high_feedback" value="2500"/>
                 <field name="timeout" value="3"/>
             </field>
         </define>
@@ -395,7 +394,8 @@
         <item name="basic law">Location, airspace and weather</item>
         <item name="RC Battery">Check the RC battery</item>
         <item name="tail connection">Check tail connection</item>
-        <item name="wing tape">Check wings taped and secured</item>
+        <item name="wing assembly">Check all four wing bolts present and tightened</item>
+        <item name="wing connection">Check wing power and CAN lines connected</item>
         <item name="inspection">Inspect airframe condition</item>
         <item name="attitude">Check attitude and heading</item>
         <item name="airspeed">Airspeed sensor calibration</item>
@@ -403,15 +403,14 @@
         <item name="actuators">Automated actuator check</item>
         <item name="flight plan">Check flight plan</item>
         <item name="flight block">Switch to correct flight block</item>
-        <item name="drone tag">Switch on drone tag</item>
         <item name="camera">Switch on camera</item>
         <item name="announce">Announce flight to other airspace users</item>
     </checklist>
 
     <section name="BAT">
         <define name="CATASTROPHIC_BAT_LEVEL"   value="18.0" unit="V"/>
-        <define name="CRITIC_BAT_LEVEL"         value="18.6" unit="V"/>
-        <define name="LOW_BAT_LEVEL"            value="19.2" unit="V"/>
+        <define name="CRITIC_BAT_LEVEL"         value="21.3" unit="V"/>
+        <define name="LOW_BAT_LEVEL"            value="22.08" unit="V"/>
         <define name="MAX_BAT_LEVEL"            value="25.2" unit="V"/>
         <define name="TAKEOFF_BAT_LEVEL"        value="24.2" unit="V"/>
         <define name="BAT_NB_CELLS"             value="6"/>
diff --git a/conf/airframes/tudelft/rotwing_v3d.xml b/conf/airframes/tudelft/rotwing_v3d.xml
index cfe86ed57c..e2839946ea 100644
--- a/conf/airframes/tudelft/rotwing_v3d.xml
+++ b/conf/airframes/tudelft/rotwing_v3d.xml
@@ -457,8 +457,8 @@
 
     <section name="BAT">
         <define name="CATASTROPHIC_BAT_LEVEL"   value="18.0" unit="V"/>
-        <define name="CRITIC_BAT_LEVEL"         value="18.6" unit="V"/>
-        <define name="LOW_BAT_LEVEL"            value="19.2" unit="V"/>
+        <define name="CRITIC_BAT_LEVEL"         value="21.3" unit="V"/>
+        <define name="LOW_BAT_LEVEL"            value="22.08" unit="V"/>
         <define name="MAX_BAT_LEVEL"            value="25.2" unit="V"/>
         <define name="TAKEOFF_BAT_LEVEL"        value="24.2" unit="V"/>
         <define name="BAT_NB_CELLS"             value="6"/>
diff --git a/conf/airframes/tudelft/rotwing_v3e.xml b/conf/airframes/tudelft/rotwing_v3e.xml
index b3c5f0668b..dee2968ee0 100644
--- a/conf/airframes/tudelft/rotwing_v3e.xml
+++ b/conf/airframes/tudelft/rotwing_v3e.xml
@@ -419,8 +419,8 @@
 
     <section name="BAT">
         <define name="CATASTROPHIC_BAT_LEVEL"   value="18.0" unit="V"/>
-        <define name="CRITIC_BAT_LEVEL"         value="18.6" unit="V"/>
-        <define name="LOW_BAT_LEVEL"            value="19.2" unit="V"/>
+        <define name="CRITIC_BAT_LEVEL"         value="21.3" unit="V"/>
+        <define name="LOW_BAT_LEVEL"            value="22.08" unit="V"/>
         <define name="MAX_BAT_LEVEL"            value="25.2" unit="V"/>
         <define name="TAKEOFF_BAT_LEVEL"        value="24.2" unit="V"/>
         <define name="BAT_NB_CELLS"             value="6"/>
diff --git a/conf/airframes/tudelft/rotwing_v3f.xml b/conf/airframes/tudelft/rotwing_v3f.xml
index 2b1b72b58d..9b94108ceb 100644
--- a/conf/airframes/tudelft/rotwing_v3f.xml
+++ b/conf/airframes/tudelft/rotwing_v3f.xml
@@ -403,8 +403,8 @@
 
     <section name="BAT">
         <define name="CATASTROPHIC_BAT_LEVEL"   value="18.0" unit="V"/>
-        <define name="CRITIC_BAT_LEVEL"         value="18.6" unit="V"/>
-        <define name="LOW_BAT_LEVEL"            value="19.2" unit="V"/>
+        <define name="CRITIC_BAT_LEVEL"         value="21.3" unit="V"/>
+        <define name="LOW_BAT_LEVEL"            value="22.08" unit="V"/>
         <define name="MAX_BAT_LEVEL"            value="25.2" unit="V"/>
         <define name="TAKEOFF_BAT_LEVEL"        value="24.2" unit="V"/>
         <define name="BAT_NB_CELLS"             value="6"/>
diff --git a/conf/airframes/tudelft/rotwing_v3g.xml b/conf/airframes/tudelft/rotwing_v3g.xml
index 571c947792..c0c1b52337 100644
--- a/conf/airframes/tudelft/rotwing_v3g.xml
+++ b/conf/airframes/tudelft/rotwing_v3g.xml
@@ -416,8 +416,8 @@
 
     <section name="BAT">
         <define name="CATASTROPHIC_BAT_LEVEL"   value="18.0" unit="V"/>
-        <define name="CRITIC_BAT_LEVEL"         value="18.6" unit="V"/>
-        <define name="LOW_BAT_LEVEL"            value="19.2" unit="V"/>
+        <define name="CRITIC_BAT_LEVEL"         value="21.3" unit="V"/>
+        <define name="LOW_BAT_LEVEL"            value="22.08" unit="V"/>
         <define name="MAX_BAT_LEVEL"            value="25.2" unit="V"/>
         <define name="TAKEOFF_BAT_LEVEL"        value="24.2" unit="V"/>
         <define name="BAT_NB_CELLS"             value="6"/>
diff --git a/conf/airframes/tudelft/rotwing_v3h.xml b/conf/airframes/tudelft/rotwing_v3h.xml
index 7eca142ecf..9dd7d26bd5 100644
--- a/conf/airframes/tudelft/rotwing_v3h.xml
+++ b/conf/airframes/tudelft/rotwing_v3h.xml
@@ -71,7 +71,7 @@
         </module>
         <module name="airspeed" type="ms45xx_i2c">
             <configure name="MS45XX_I2C_DEV"            value="i2c2"/>
-            <define name="MS45XX_PRESSURE_SCALE"        value="1.90"/>
+            <define name="MS45XX_PRESSURE_SCALE"        value="1.6580"/>
             <define name="USE_AIRSPEED_LOWPASS_FILTER"  value="TRUE"/>
             <define name="MS45XX_LOWPASS_TAU"           value="0.25"/>
             <define name="AIRSPEED_MS45XX_SEND_ABI"     value="1"/>
@@ -403,8 +403,8 @@
 
     <section name="BAT">
         <define name="CATASTROPHIC_BAT_LEVEL"   value="18.0" unit="V"/>
-        <define name="CRITIC_BAT_LEVEL"         value="18.6" unit="V"/>
-        <define name="LOW_BAT_LEVEL"            value="19.2" unit="V"/>
+        <define name="CRITIC_BAT_LEVEL"         value="21.3" unit="V"/>
+        <define name="LOW_BAT_LEVEL"            value="22.08" unit="V"/>
         <define name="MAX_BAT_LEVEL"            value="25.2" unit="V"/>
         <define name="TAKEOFF_BAT_LEVEL"        value="24.2" unit="V"/>
         <define name="BAT_NB_CELLS"             value="6"/>
diff --git a/conf/airframes/tudelft/rotwing_v3i.xml b/conf/airframes/tudelft/rotwing_v3i.xml
index 739be0c00d..7c714a4941 100644
--- a/conf/airframes/tudelft/rotwing_v3i.xml
+++ b/conf/airframes/tudelft/rotwing_v3i.xml
@@ -452,8 +452,8 @@
 
     <section name="BAT">
         <define name="CATASTROPHIC_BAT_LEVEL"   value="18.0" unit="V"/>
-        <define name="CRITIC_BAT_LEVEL"         value="18.6" unit="V"/>
-        <define name="LOW_BAT_LEVEL"            value="19.2" unit="V"/>
+        <define name="CRITIC_BAT_LEVEL"         value="21.3" unit="V"/>
+        <define name="LOW_BAT_LEVEL"            value="22.08" unit="V"/>
         <define name="MAX_BAT_LEVEL"            value="25.2" unit="V"/>
         <define name="TAKEOFF_BAT_LEVEL"        value="24.2" unit="V"/>
         <define name="BAT_NB_CELLS"             value="6"/>
diff --git a/conf/airframes/tudelft/rotwing_v3j.xml b/conf/airframes/tudelft/rotwing_v3j.xml
index 97615f0862..8a9a7152b1 100644
--- a/conf/airframes/tudelft/rotwing_v3j.xml
+++ b/conf/airframes/tudelft/rotwing_v3j.xml
@@ -455,8 +455,8 @@
 
     <section name="BAT">
         <define name="CATASTROPHIC_BAT_LEVEL"   value="18.0" unit="V"/>
-        <define name="CRITIC_BAT_LEVEL"         value="18.6" unit="V"/>
-        <define name="LOW_BAT_LEVEL"            value="19.2" unit="V"/>
+        <define name="CRITIC_BAT_LEVEL"         value="21.3" unit="V"/>
+        <define name="LOW_BAT_LEVEL"            value="22.08" unit="V"/>
         <define name="MAX_BAT_LEVEL"            value="25.2" unit="V"/>
         <define name="TAKEOFF_BAT_LEVEL"        value="24.2" unit="V"/>
         <define name="BAT_NB_CELLS"             value="6"/>
diff --git a/conf/airframes/tudelft/rotwing_v3k.xml b/conf/airframes/tudelft/rotwing_v3k.xml
index 05610f90aa..7857fabf97 100644
--- a/conf/airframes/tudelft/rotwing_v3k.xml
+++ b/conf/airframes/tudelft/rotwing_v3k.xml
@@ -430,8 +430,8 @@
 
     <section name="BAT">
         <define name="CATASTROPHIC_BAT_LEVEL"   value="18.0" unit="V"/>
-        <define name="CRITIC_BAT_LEVEL"         value="18.6" unit="V"/>
-        <define name="LOW_BAT_LEVEL"            value="19.2" unit="V"/>
+        <define name="CRITIC_BAT_LEVEL"         value="21.3" unit="V"/>
+        <define name="LOW_BAT_LEVEL"            value="22.08" unit="V"/>
         <define name="MAX_BAT_LEVEL"            value="25.2" unit="V"/>
         <define name="TAKEOFF_BAT_LEVEL"        value="24.2" unit="V"/>
         <define name="BAT_NB_CELLS"             value="6"/>
diff --git a/conf/autopilot/rotorcraft_rotwing.xml b/conf/autopilot/rotorcraft_rotwing.xml
new file mode 100644
index 0000000000..bae377d503
--- /dev/null
+++ b/conf/autopilot/rotorcraft_rotwing.xml
@@ -0,0 +1,268 @@
+<!DOCTYPE autopilot SYSTEM "autopilot.dtd">
+
+<autopilot name="Rotating Wing Autopilot">
+
+  <state_machine name="ap" freq="PERIODIC_FREQUENCY" gcs_mode="true" settings_mode="true" settings_handler="autopilot_generated|SetModeHandler">
+
+    <includes>
+      <include name="generated/airframe.h"/>
+      <include name="autopilot.h"/>
+      <include name="autopilot_rc_helpers.h"/>
+      <include name="navigation.h"/>
+      <include name="guidance.h"/>
+      <include name="stabilization.h"/>
+      <include name="modules/radio_control/radio_control.h"/>
+      <include name="modules/gps/gps.h"/>
+      <include name="modules/actuators/actuators.h"/>
+      <include name="modules/rotwing_drone/rotwing_state.h"/>
+      <include name="modules/nav/ground_detect.h"/>
+
+      <!-- Failsafe variables -->
+      <define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20" cond="ifndef NO_GPS_LOST_WITH_DATALINK_TIME"/>
+      <define name="NO_GPS_LOST_WITH_RC_VALID" value="FALSE" cond="ifndef NO_GPS_LOST_WITH_RC_VALID"/>
+      <define name="RC_LOST_MODE" value="AP_MODE_NAV" cond="ifndef RC_LOST_MODE"/>
+      <define name="FAILSAFE_DESCENT_SPEED" value="1.0" cond="ifndef FAILSAFE_DESCENT_SPEED"/>
+
+      <!-- Modes -->
+      <define  name="MODE_MANUAL"    value="AP_MODE_ATTITUDE_DIRECT"  cond="ifndef MODE_MANUAL"/>
+      <define  name="MODE_AUTO2"     value="AP_MODE_NAV"              cond="ifndef MODE_AUTO2"/>
+      <define  name="RCLost()"       value="(radio_control.status != RC_OK)"/> <!-- !=RC_OK might be a little strict -->
+      <define  name="DLModeNav()"    value="(autopilot_mode_auto2 == AP_MODE_NAV)"/>
+      <define  name="DLModeGuided()" value="(autopilot_mode_auto2 == AP_MODE_GUIDED)"/>
+      <define  name="DLModeModule()" value="(MODE_AUTO1 == AP_MODE_MODULE)"/>
+      <define  name="DLModeAZH()"    value="(MODE_AUTO1 == AP_MODE_ATTITUDE_Z_HOLD)"/>
+      <define  name="SetThrottleValue()"    value="autopilot.throttle = commands[COMMAND_THRUST];"/>
+    </includes>
+
+    <settings>
+      <dl_setting var="autopilot.kill_throttle" min="0" step="1" max="1" module="autopilot" values="Resurrect|Kill" handler="KillThrottle"/>
+      <dl_setting var="autopilot_mode_auto2" min="2" step="1" max="3" module="autopilot" values="NAV|GUIDED"/>
+    </settings>
+
+    <control_block name="set_commands">
+      <call fun="SetRotorcraftCommands(stabilization.cmd, autopilot_in_flight(), autopilot_get_motors_on())"/>
+      <call fun="SetThrottleValue()"/>
+    </control_block>
+
+    <control_block name="run_attitude_control">
+      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+      <call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
+    </control_block>
+
+    <control_block name="run_rate_control">
+      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+      <call fun="stabilization_run(autopilot_in_flight(), &stabilization.rc_sp, &thrust_sp, stabilization.cmd)"/>
+    </control_block>
+
+    <control_block name="run_guidance_control">
+      <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+      <call fun="guidance_h_run(autopilot_in_flight())" store="struct StabilizationSetpoint stab_sp"/>
+      <call fun="stabilization_run(autopilot_in_flight(), &stab_sp, &thrust_sp, stabilization.cmd)"/>
+    </control_block>
+
+    <control_block name="update_v_sp_gcs">
+      <call fun="guidance_v_set_z(-nav.nav_altitude)"/>
+      <call fun="guidance_h_set_pos(nav.carrot.y, nav.carrot.x)"/>
+    </control_block>
+
+    <exceptions>
+      <exception cond="nav.too_far_from_home" deroute="HOME"/>
+      <exception cond="kill_switch_is_on()" deroute="KILL"/>
+    </exceptions>
+
+    <!-- Kill throttle mode 0-->
+    <mode name="KILL">
+      <select cond="kill_switch_is_on()"/>
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_NONE, 0)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_KILL)"/>
+        <call fun="autopilot_set_in_flight(false)"/>
+        <call fun="autopilot_set_motors_on(false)"/>
+        <call fun="stabilization.cmd[COMMAND_THRUST] = 0"/>
+      </on_enter>
+      <control>
+        <call fun="SetCommands(commands_failsafe)"/>
+      </control>
+    </mode>
+
+    <!-- Failsafe mode 1-->
+    <mode name="FAILSAFE" shortname="FAIL">
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_CLIMB)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
+        <call fun="guidance_v_set_vz(FAILSAFE_DESCENT_SPEED)"/>
+        <call fun="NavStartDetectGround()"/>
+      </on_enter>
+      <control>
+        <call fun="NavVerticalThrottleMode(MAX_PPRZ*(0.5))"/>
+        <call fun="stabilization_get_failsafe_sp()" store="struct StabilizationSetpoint stab_failsafe"/>
+        <call fun="guidance_v_run(autopilot_in_flight())" store="struct ThrustSetpoint thrust_sp"/>
+        <call fun="stabilization_run(autopilot_in_flight(), &stab_failsafe, &thrust_sp, stabilization.cmd)"/>
+        <call_block name="set_commands"/>
+      </control>
+      <exception cond="!GpsIsLost() && !(agl_dist_valid && agl_dist_value_filtered < 5.0)" deroute="$LAST_MODE"/>
+      <exception cond="NavDetectGround()" deroute="Kill"/>
+      <exception cond="!nav_is_in_flight()" deroute="Kill"/>
+      <exception cond="ground_detect()" deroute="Kill"/>
+    </mode>
+
+    <!-- Home mode 2-->
+    <mode name="HOME">
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
+      </on_enter>
+      <control freq="NAVIGATION_FREQUENCY">
+        <call fun="nav_home()"/>
+      </control>
+      <control>
+        <call_block name="run_guidance_control"/>
+        <call_block name="set_commands"/>
+      </control>
+      <exception cond="GpsIsLost() && autopilot_in_flight() && !NO_GPS_LOST_WITH_RC_VALID && (datalink_time > NO_GPS_LOST_WITH_DATALINK_TIME)" deroute="FAILSAFE"/>
+    </mode>
+
+    <!-- Rate mode 3-->
+    <mode name="RATE_DIRECT" shortname="RATE">
+      <!-- <select cond=""/> -->
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_RATE, 0)"/>
+      </on_enter>
+      <control freq="NAVIGATION_FREQUENCY">
+        <call fun="nav_periodic_task()"/>
+      </control>
+      <control>
+        <call_block name="run_rate_control"/>
+        <call_block name="set_commands"/>
+      </control>
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- Attitude mode 4-->
+    <mode name="ATTITUDE_DIRECT" shortname="ATT_QUAD">
+      <select cond="RCMode0()"/>
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
+      </on_enter>
+      <control freq="NAVIGATION_FREQUENCY">
+        <call fun="nav_periodic_task()"/>
+      </control>
+      <control>
+        <call_block name="run_attitude_control"/>
+        <call_block name="set_commands"/>
+      </control>
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- Rate climb rc mode 5-->
+    <mode name="RATE_RC_CLIMB" shortname="RRCC">
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- ATTITUDE_RC_CLIMB: 6 -->
+    <mode name="ATTITUDE_RC_CLIMB" shortname="ATTITUDE_RC_CLIMB">
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- ATTITUDE_CLIMB: 7 -->
+    <mode name="ATTITUDE_CLIMB" shortname="ATTITUDE_CLIMB">
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- RATE_Z_HOLD: 8 -->
+    <mode name="RATE_Z_HOLD" shortname="RATE_Z_HOLD">
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- ATTITUDE_Z_HOLD: 9 -->
+    <mode name="ATTITUDE_Z_HOLD" shortname="A_ZH">
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- HOVER_DIRECT: 10 -->
+    <mode name="HOVER_DIRECT" shortname="HOVER_DIRECT">
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- HOVER_CLIMB: 11 -->
+    <mode name="HOVER_CLIMB" shortname="HOVER_CLIMB">
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- HOVER_Z_HOLD: 12 -->
+    <mode name="HOVER_Z_HOLD" shortname="HOVER_Z_HOLD">
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- NAV: 13 -->
+    <mode name="NAV">
+      <select cond="RCMode2() && DLModeNav()" exception="HOME"/>
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NAV)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_NAV)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_HEADING)"/>
+      </on_enter>
+      <control freq="NAVIGATION_FREQUENCY">
+        <call fun="nav_periodic_task()"/>
+        
+      </control>
+      <control>
+        <call_block name="update_v_sp_gcs"/>
+        <call_block name="run_guidance_control"/>
+        <call_block name="set_commands"/>
+      </control>
+      <exception cond="GpsIsLost() && autopilot_in_flight() && !NO_GPS_LOST_WITH_RC_VALID && (datalink_time > NO_GPS_LOST_WITH_DATALINK_TIME)" deroute="FAILSAFE"/>
+    </mode>
+
+        <!-- RC_DIRECT: 14 -->
+    <mode name="RC_DIRECT" shortname="RC_DIRECT">
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- CARE_FREE_DIRECT: 15 -->
+    <mode name="CARE_FREE_DIRECT" shortname="CARE_FREE_DIRECT">
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- FORWARD: 16 -->
+    <mode name="FORWARD" shortname="FW">
+      <select cond="RCMode1()"/>
+      <on_enter>
+        <call fun="guidance_h_mode_changed(GUIDANCE_H_MODE_NONE)"/>
+        <call fun="guidance_v_mode_changed(GUIDANCE_V_MODE_RC_DIRECT)"/>
+        <call fun="stabilization_mode_changed(STABILIZATION_MODE_ATTITUDE, STABILIZATION_ATT_SUBMODE_FORWARD)"/>
+      </on_enter>
+      <control freq="NAVIGATION_FREQUENCY">
+        <call fun="nav_periodic_task()"/>
+      </control>
+      <control>
+        <call_block name="run_attitude_control"/>
+        <call_block name="set_commands"/>
+      </control>
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- MODULE: 17 -->
+    <mode name="MODULE" shortname="MODULE">
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- FLIP: 18 -->
+    <mode name="FLIP" shortname="FLIP">
+      <exception cond="RCLost()" deroute="NAV"/>
+    </mode>
+
+    <!-- GUIDED: 19 -->
+    <mode name="GUIDED">
+      <exception cond="GpsIsLost() && autopilot_in_flight() && !NO_GPS_LOST_WITH_RC_VALID && (datalink_time > NO_GPS_LOST_WITH_DATALINK_TIME)" deroute="FAILSAFE"/>
+    </mode>  
+  </state_machine>
+
+</autopilot>
\ No newline at end of file
diff --git a/conf/flight_plans/tudelft/rotwing_EHVB.xml b/conf/flight_plans/tudelft/rotwing_EHVB.xml
index f8b4c0bf1c..dc5bc95e7d 100644
--- a/conf/flight_plans/tudelft/rotwing_EHVB.xml
+++ b/conf/flight_plans/tudelft/rotwing_EHVB.xml
@@ -133,31 +133,32 @@
       <call_once fun="nav_set_heading_current()"/>
       <stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
     </block>
-    <block name="Standby" strip_button="Standby" strip_icon="home.png" pre_call="rotwing_state_choose_state_by_dist(WP_STDBY)">
-        <stay wp="STDBY"/>
+    <block name="Standby" strip_button="Standby" strip_icon="home.png">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+      <stay wp="STDBY"/>
     </block>
     <block name="Standby_dist" strip_button="Standby Dist" strip_icon="home.png" pre_call="rotwing_state_choose_state_by_dist(WP_STDBY)">
-        <stay wp="STDBY"/>
+      <stay wp="STDBY"/>
     </block>
     <block name="Standby_free" strip_button="Standby Free" strip_icon="home.png">
-        <call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
-        <stay wp="STDBY"/>
+      <call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
+      <stay wp="STDBY"/>
     </block>
     <block name="stay_p1" strip_button="Stay P1" strip_icon="wp_quad.png">
-        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
-        <stay wp="p1"/>
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+      <stay wp="p1"/>
     </block>
     <block name="stay_p2" strip_button="Stay P2" strip_icon="wp_quad.png">
-        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
-        <stay wp="p2"/>
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+      <stay wp="p2"/>
     </block>
     <block name="stay_p3" strip_button="Stay P3" strip_icon="wp_quad.png">
-        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
-        <stay wp="p3"/>
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+      <stay wp="p3"/>
     </block>
     <block name="stay_p4" strip_button="Stay P4" strip_icon="wp_quad.png">
-        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
-        <stay wp="p4"/>
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+      <stay wp="p4"/>
     </block>
     <!-- <block name="Approach APP">
       <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
@@ -209,35 +210,35 @@
       <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
     </block> -->
     <block name="land here">
-        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
-        <call_once fun="NavSetWaypointHere(WP_TD)"/>
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+      <call_once fun="NavSetWaypointHere(WP_TD)"/>
     </block>
     <block name="land" strip_button="Land" strip_icon="land-right.png">
-        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
-        <go wp="TD"/>
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+      <go wp="TD"/>
     </block>
     <block name="descend" strip_button="Descend" strip_icon="descend.png">
-        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
-        <exception cond="GetPosHeight() @LT flare_height" deroute="flare"/>
-        <stay climb="-1.0" vmode="climb" wp="TD"/>
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+      <exception cond="GetPosHeight() @LT flare_height" deroute="flare"/>
+      <stay climb="-1.0" vmode="climb" wp="TD"/>
     </block>
     <block name="flare">
-        <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
-        <stay climb="-0.5" vmode="climb" wp="TD"/>
-        <!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>-->
-        <exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/>
+      <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+      <stay climb="-0.5" vmode="climb" wp="TD"/>
+      <!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>-->
+      <exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/>
     </block>
     <block name="flare_low">
-        <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
-        <!-- <exception cond="NavDetectGround()" deroute="Holding point"/> -->
-        <exception cond="!nav_is_in_flight()" deroute="Holding point"/>
-        <exception cond="ground_detect()" deroute="Holding point"/>
-        <!-- <call_once fun="NavStartDetectGround()"/> -->
-        <stay climb="-0.5" vmode="climb" wp="TD"/>
+      <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+      <!-- <exception cond="NavDetectGround()" deroute="Holding point"/> -->
+      <exception cond="!nav_is_in_flight()" deroute="Holding point"/>
+      <exception cond="ground_detect()" deroute="Holding point"/>
+      <!-- <call_once fun="NavStartDetectGround()"/> -->
+      <stay climb="-0.5" vmode="climb" wp="TD"/>
     </block>
     <block name="landed">
-        <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
-        <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+      <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
     </block>
   </blocks>
 </flight_plan>
diff --git a/conf/flight_plans/tudelft/rotwing_generic.xml b/conf/flight_plans/tudelft/rotwing_generic.xml
new file mode 100644
index 0000000000..6debb93d54
--- /dev/null
+++ b/conf/flight_plans/tudelft/rotwing_generic.xml
@@ -0,0 +1,235 @@
+<!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
+<flight_plan alt="50" ground_alt="0" lat0="52.962558" lon0="4.793334" max_dist_from_home="5000" name="Rotating Wing Generic" security_height="2">
+  <header/>
+  <waypoints>
+    <waypoint name="HOME" x="0" y="0"/>
+    <waypoint name="CLIMB" x="300." y="70"/>
+    <waypoint name="STDBY" x="60" y="20"/>
+    <waypoint name="p1" x="65" y="75"/>
+    <waypoint name="p2" x="200" y="120"/>
+    <waypoint name="p3" x="250" y="-30"/>
+    <waypoint name="p4" x="120" y="-75"/>
+    <waypoint name="circ" x="160" y="25"/>
+    <waypoint name="TD" x="10" y="-1"/>
+    <!-- <waypoint name="APP" x="70" y="-25"/>
+    <waypoint name="FOLLOW" x="300" y="80"/> -->
+    <!-- t'Harde -->
+    <waypoint x="1500" y="0" name="C1"/>
+    <waypoint x="750" y="-1299" name="C2"/>
+    <waypoint x="-750" y="-1299" name="C3"/>
+    <waypoint x="-1500" y="0" name="C4"/>
+    <waypoint x="-750" y="1299" name="C5"/>
+    <waypoint x="750" y="1299" name="C6"/>
+
+    <waypoint x="1000" y="0" name="S1"/>
+    <waypoint x="500" y="-866" name="S2"/>
+    <waypoint x="-500" y="-866" name="S3"/>
+    <waypoint x="-1000" y="0" name="S4"/>
+    <waypoint x="-500" y="866" name="S5"/>
+    <waypoint x="500" y="866" name="S6"/>
+  </waypoints>
+  <sectors>
+    <sector color="red" name="HardGeofence">
+      <corner name="C1"/>
+      <corner name="C2"/>
+      <corner name="C3"/>
+      <corner name="C4"/>
+      <corner name="C5"/>
+      <corner name="C6"/>
+    </sector>
+    <sector color="orange" name="SoftGeofence">
+      <corner name="S1"/>
+      <corner name="S2"/>
+      <corner name="S3"/>
+      <corner name="S4"/>
+      <corner name="S5"/>
+      <corner name="S6"/>
+    </sector>
+  </sectors>
+  <variables>
+    <variable var="liftoff_pitch"/>
+    <variable var="liftoff_roll"/>
+    <variable var="liftoff_pitch_limit" init="LIFTOFF_PITCH_LIMIT_DEG"/>
+    <variable var="liftoff_roll_limit" init="LIFTOFF_ROLL_LIMIT_DEG"/>
+    <variable var="stage_timer_msec"/>
+    <variable var="hybrid_height" init="HYBRID_HEIGHT"/>
+    <variable var="transition_height" init="TRANSITION_HEIGHT"/>
+    <variable var="flare_height" init="FLARE_HEIGHT"/>
+    <variable var="geofence_soft_height" init="250" min="0" step="10" max="5000" shortname="geofence_soft_height"/>
+    <variable var="geofence_hard_height" init="500" min="0" step="10" max="5000" shortname="geofence_hard_height"/>
+  </variables>
+  <modules>
+    <module name="follow_me">
+      <define name="FOLLOW_ME_MOVING_WPS" value="WP_STDBY,WP_circ,WP_TD"/>
+    </module>
+  </modules>
+
+  <exceptions>
+    <!-- Outside of hard (red) geofence kill -->
+    <exception cond="Or(!InsideHardGeofence(GetPosX(), GetPosY()), GetPosHeight() @GT geofence_hard_height) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point'))" deroute="Holding point"/>
+    <!-- Outside of soft (orange) geofence go to standby-->
+    <exception cond="Or(!InsideSoftGeofence(GetPosX(), GetPosY()), GetPosHeight() @GT geofence_soft_height) @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
+    <!-- Datalink timeout go to standby free -->
+    <exception cond="datalink_time @GT 25 @AND !(nav_block == IndexOfBlock('Wait GPS')) @AND !(nav_block == IndexOfBlock('Geo init')) @AND !(nav_block == IndexOfBlock('Holding point')) @AND !(nav_block == IndexOfBlock('Standby'))" deroute="Standby"/>
+    <!-- Bat low return to standby-->
+    <exception cond="(electrical.bat_low @AND !(IndexOfBlock('Holding point') @GT nav_block) @AND !(nav_block >= IndexOfBlock('land here')) @AND (autopilot_in_flight() == true) )" deroute="Standby"/>
+    <!-- Bat critical land here-->
+    <exception cond="(electrical.bat_critical @AND !(IndexOfBlock('Holding point') @GT nav_block) @AND !(nav_block >= IndexOfBlock('land here')) @AND (autopilot_in_flight() == true) )" deroute="land here"/>
+  </exceptions>
+  <blocks>
+   <block name="Wait GPS">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+      <call_once fun="NavKillThrottle()"/>
+      <while cond="!GpsFixValid() || !state.ned_initialized_i"/>
+    </block>
+    <block name="Geo init">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+      <while cond="LessThan(NavBlockTime(), 10)"/>
+      <call_once fun="NavSetGroundReferenceHere()"/>
+    </block>
+    <block name="Holding point">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+      <call_once fun="NavKillThrottle()"/>
+      <set var="stabilization.cmd[COMMAND_THRUST_X]" value="0"/>
+      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
+    <block name="Start Engine" strip_button="Take-off" strip_icon="on.png">
+        <exception cond="stage_time > 10" deroute="Holding point"/>
+        <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+        <call_once fun="nav_set_heading_current()"/>
+        <call_once fun="NavResurrect()"/>
+        <attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0" until="rotwing_state_hover_motors_running()" vmode="throttle"/>
+    </block>
+    <block name="Wait takeoff" strip_button="Levelling Check">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+      <exception cond="stage_time > 10" deroute="Holding point"/>
+      <attitude pitch="DegOfRad(stateGetNedToBodyEulers_f()->theta)" roll="DegOfRad(stateGetNedToBodyEulers_f()->phi)" throttle="0.2" until="(fabs(DegOfRad(stateGetNedToBodyEulers_f()->theta)) @LT liftoff_roll_limit) @AND (fabs(DegOfRad(stateGetNedToBodyEulers_f()->phi)) @LT liftoff_pitch_limit) @AND (stage_time > 2)" vmode="throttle"/>
+    </block>
+    <block name="Takeoff" strip_button="Lift-off" strip_icon="takeoff.png">
+        <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+        <exception cond="agl_dist_valid @AND (agl_dist_value @GT 1.0)" deroute="Climb"/>
+        <call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
+        <set var="stage_timer_msec" value="get_sys_time_msec()"/>
+        <set var="liftoff_roll" value="DegOfRad(stateGetNedToBodyEulers_f()->phi)"/>
+        <set var="liftoff_pitch" value="DegOfRad(stateGetNedToBodyEulers_f()->theta)"/>
+        <attitude pitch="liftoff_pitch" roll="liftoff_roll" throttle="0.75" until="(get_sys_time_msec()-stage_timer_msec)>250" vmode="throttle"/>
+        <call_once fun="autopilot_set_in_flight(true)"/><!-- this is a hack to solve INDI not being active fast enough -->
+
+        <call_once fun="NavSetWaypointHere(WP_CLIMB)"/>
+        <attitude pitch="0" roll="0" throttle="0.75" until="stage_time>3" vmode="throttle"/>
+    </block>
+    <block name="Climb">
+      <exception cond="GetPosHeight() @GT hybrid_height" deroute="Standby"/>
+      <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+      <call_once fun="nav_set_heading_current()"/>
+      <stay climb="nav.climb_vspeed" vmode="climb" wp="CLIMB"/>
+    </block>
+    <block name="Standby" strip_button="Standby" strip_icon="home.png" pre_call="rotwing_state_choose_state_by_dist(WP_STDBY)">
+        <stay wp="STDBY"/>
+    </block>
+    <block name="Standby_free" strip_button="Standby Free" strip_icon="home.png">
+        <call_once fun="rotwing_state_set(ROTWING_STATE_FREE)"/>
+        <stay wp="STDBY"/>
+    </block>
+    <block name="stay_p1" strip_button="Stay P1" strip_icon="wp_quad.png">
+        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+        <stay wp="p1"/>
+    </block>
+    <block name="stay_p2" strip_button="Stay P2" strip_icon="wp_quad.png">
+        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+        <stay wp="p2"/>
+    </block>
+    <block name="stay_p3" strip_button="Stay P3" strip_icon="wp_quad.png">
+        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+        <stay wp="p3"/>
+    </block>
+    <block name="stay_p4" strip_button="Stay P4" strip_icon="wp_quad.png">
+        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+        <stay wp="p4"/>
+    </block>
+    <!-- <block name="Approach APP">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+      <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
+    </block> -->
+    <!-- <block name="follow_module">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+      <stay pre_call="follow_me_set_wp(WP_p1, 0)" wp="p1"/>
+    </block> -->
+    <!-- <block name="route">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
+      <go from="p1" hmode="route" wp="p2"/>
+      <go from="p2" hmode="route" wp="p3"/>
+      <go from="p3" hmode="route" wp="p4"/>
+      <go from="p4" hmode="route" wp="p1"/>
+      <deroute block="route"/>
+    </block> -->
+    <block name="route_p1" strip_button="Route P1" strip_icon="path.png">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
+      <go wp="p1"/>
+    </block>
+    <block name="route_p2" strip_button="Route P2" strip_icon="path.png">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
+      <go wp="p2"/>
+    </block>
+    <block name="route_p3" strip_button="Route P3" strip_icon="path.png">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
+      <go wp="p3"/>
+    </block>
+    <block name="route_p4" strip_button="Route P4" strip_icon="path.png">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
+      <go wp="p4"/>
+      <deroute block="route_p1"/>
+    </block>
+    <block name="Circle_CW_fwd" strip_button="Circle CW" strip_icon="circle-right.png">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
+      <circle radius="nav.radius" wp="circ"/>
+    </block>
+    <block name="Circle_CCW_fwd" strip_button="Circle CCW" strip_icon="circle-left.png">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
+      <circle radius="-nav.radius" wp="circ"/>
+    </block>
+    <block name="Circle_Auto_fwd" strip_button="Circle Auto" strip_icon="circle-left.png"> <!-- Comment out later -->
+      <exception cond="!rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CW_fwd"/>
+      <exception cond="rotwing_state_choose_circle_direction(WP_circ)" deroute="Circle_CCW_fwd"/>
+    </block>
+    <!-- <block name="Survey S1-S2 25m NS" strip_button="Sweep North South p1-p2">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_FW)"/>
+      <survey_rectangle grid="100" orientation="NS" wp1="p1" wp2="p2"/>
+    </block> -->
+    <!-- <block name="Approach APP">
+      <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+      <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
+    </block> -->
+    <block name="land here">
+        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+        <call_once fun="NavSetWaypointHere(WP_TD)"/>
+    </block>
+    <block name="land" strip_button="Land" strip_icon="land-right.png">
+        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+        <go wp="TD"/>
+    </block>
+    <block name="descend" strip_button="Descend" strip_icon="descend.png">
+        <call_once fun="rotwing_state_set(ROTWING_STATE_REQUEST_HOVER)"/>
+        <exception cond="GetPosHeight() @LT flare_height" deroute="flare"/>
+        <stay climb="-1.0" vmode="climb" wp="TD"/>
+    </block>
+    <block name="flare">
+        <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+        <stay climb="-0.5" vmode="climb" wp="TD"/>
+        <!--<exception cond="!(GetPosHeight() @LT 2.0)" deroute="flare_low"/>-->
+        <exception cond="agl_dist_valid @AND (agl_dist_value @LT 0.28)" deroute="flare_low"/>
+    </block>
+    <block name="flare_low">
+        <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+        <exception cond="NavDetectGround()" deroute="Holding point"/>
+        <exception cond="!nav_is_in_flight()" deroute="Holding point"/>
+        <exception cond="ground_detect()" deroute="Holding point"/>
+        <call_once fun="NavStartDetectGround()"/>
+        <stay climb="-0.5" vmode="climb" wp="TD"/>
+    </block>
+    <block name="landed">
+        <call_once fun="rotwing_state_set(ROTWING_STATE_FORCE_HOVER)"/>
+        <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+    </block>
+  </blocks>
+</flight_plan>
\ No newline at end of file
diff --git a/conf/joystick/tudelft/rotwing_taranis_x9d_plus.xml b/conf/joystick/tudelft/rotwing_taranis_x9d_plus.xml
new file mode 100644
index 0000000000..158f2f33b2
--- /dev/null
+++ b/conf/joystick/tudelft/rotwing_taranis_x9d_plus.xml
@@ -0,0 +1,43 @@
+<!--
+FrSky Tarranis X9D plus over USB (mode 2)
+http://www.frsky-rc.com/product/pro.php?pro_id=137
+
+In this file we will use it as a 6CH joystick to control a UAS.
+- The left stick vertical axis will be used for throttle
+- The right stick horizontal axis will be used for roll
+- The right stick vertical axis will be used for pitch
+- The left stick horizontal axis will be used for yaw
+- The VRA axis will be used for mode switching (right top switch)
+- The arm axis will be used for arming (left top switch)
+- The pusher can be controlled using the slider on the right
+
+If you want to fly your UAS via the joystick add this to your session:
+
+ /home/username/paparazzi/sw/ground_segment/joystick/input2ivy  -d 0 -ac yourarfamename tudelft/Rotwing_Taranis_X9D_plus.xml
+
+Where -d 0 must be -d 1 if you have a laptop with accelometer installed
+
+The basis of steering is the standard signs of aerospace convention
+-->
+
+<joystick>
+  <input>
+    <axis index="3" name="LeftStickHorizontal"/>
+    <axis index="0" name="LeftStickVertical"/>
+    <axis index="1" name="RightStickHorizontal"/>
+    <axis index="2" name="RightStickVertical"/>
+    <axis index="4" name="VRA"/>
+    <axis index="5" name="arm"/>
+    <axis index="6" name="yellow"/>
+    <button index="0" name="thrust_x"/> <!-- pusher doesn't work well due to it being treated as a button, only zero or full throttle work -->
+    <button index="1" name="filler"/> <!-- necessary to fill the channels array such that pusher can be in the righ -->
+  </input>
+
+<!-- Follow the order of rc_datalink.h -->
+  <messages period="0.0333333">
+    <message class="datalink" name="RC_UP" send_always="true">
+      <field name="channels" value="RightStickHorizontal;RightStickVertical;LeftStickHorizontal;Fit(LeftStickVertical,-127,127,0,127);VRA;arm;yellow;filler;Fit(thrust_x,0,1,-127,127)"/>
+    </message>
+  </messages>
+
+</joystick>
\ No newline at end of file
diff --git a/conf/modules/math.xml b/conf/modules/math.xml
index 41f9ad1505..9d5110d20c 100644
--- a/conf/modules/math.xml
+++ b/conf/modules/math.xml
@@ -24,6 +24,7 @@
     <file name="pprz_trig_int.h" dir="math"/>
     <file name="pprz_orientation_conversion.h" dir="math"/>
     <file name="pprz_stat.h"/>
+    <file name="pprz_random.h" dir="math"/>
   </header>
   <init fun="pprz_trig_int_init()"/>
   <makefile>
@@ -36,7 +37,7 @@
     <file name="pprz_trig_int.c" dir="math"/>
     <file name="pprz_orientation_conversion.c" dir="math"/>
     <file name="pprz_stat.c" dir="math"/>
+    <file name="pprz_random.c" dir="math"/>
     <test/>
   </makefile>
 </module>
-
diff --git a/conf/modules/rotwing_state.xml b/conf/modules/rotwing_state.xml
index 9af4f76bb8..2fa1399479 100644
--- a/conf/modules/rotwing_state.xml
+++ b/conf/modules/rotwing_state.xml
@@ -12,6 +12,7 @@
         <dl_setting var="rotwing_state.nav_state" min="0" max="4" step="1" values="FORCE_HOVER|REQ_HOVER|FORCE_FW|REQ_FW|FREE" shortname="nav_state"/>
         <dl_setting var="rotwing_state.fw_min_airspeed" min="0" max="50" step="0.1" shortname="fw_min_airspeed"/>
         <dl_setting var="rotwing_state.cruise_airspeed" min="0" max="50" step="0.1" shortname="cruise_airspeed"/>
+        <dl_setting var="nav.radius" min="50" step="1" max="1000" unit="m" shortname="circle_radius"></dl_setting>
         <dl_setting var="rotwing_state.force_skew" min="0" max="1" step="1" values="FALSE|TRUE" shortname="force_skew"/>
         <dl_setting var="rotwing_state.sp_skew_angle_deg" min="0" max="90" step="1" shortname="sp_skew_angle"/>
         <dl_setting var="rotwing_state.fail_skew_angle" min="0" max="1" step="1" values="OFF|ON" shortname="fail_skew"/>
diff --git a/conf/simulator/jsbsim/aircraft/rotwing3.xml b/conf/simulator/jsbsim/aircraft/rotwing3.xml
index 2dbaafe22e..eb9c60cf74 100644
--- a/conf/simulator/jsbsim/aircraft/rotwing3.xml
+++ b/conf/simulator/jsbsim/aircraft/rotwing3.xml
@@ -121,47 +121,47 @@
       <!--First order filter represents actuator dynamics-->
       <lag_filter name="front_motor_lag">
         <input> fcs/front_motor </input>
-        <c1> 18 </c1>
+        <c1> 22.0 </c1>
         <output> fcs/front_motor_lag</output>
       </lag_filter>
       <lag_filter name="right_motor_lag">
         <input> fcs/right_motor </input>
-        <c1> 18 </c1>
+        <c1> 22.0 </c1>
         <output> fcs/right_motor_lag</output>
       </lag_filter>
       <lag_filter name="back_motor_lag">
         <input> fcs/back_motor </input>
-        <c1> 18 </c1>
+        <c1> 22.0 </c1>
         <output> fcs/back_motor_lag</output>
       </lag_filter>
       <lag_filter name="left_motor_lag">
         <input> fcs/left_motor </input>
-        <c1> 18 </c1>
+        <c1> 22.0 </c1>
         <output> fcs/left_motor_lag</output>
       </lag_filter>
       <lag_filter name="pusher_lag">
         <input> fcs/pusher </input>
-        <c1> 18 </c1>
+        <c1> 30.0 </c1>
         <output> fcs/pusher_lag</output>
       </lag_filter>
       <lag_filter name="elevator_lag">
         <input> fcs/elevator </input>
-        <c1> 54 </c1>
+        <c1> 50.0 </c1>
         <output> fcs/elevator_lag</output>
       </lag_filter>
       <lag_filter name="rudder_lag">
         <input> fcs/rudder </input>
-        <c1> 54 </c1>
+        <c1> 50.0 </c1>
         <output> fcs/rudder_lag</output>
       </lag_filter>
       <lag_filter name="aileron_lag">
         <input> fcs/aileron </input>
-        <c1> 54 </c1>
+        <c1> 50.0 </c1>
         <output> fcs/aileron_lag</output>
       </lag_filter>
       <lag_filter name="flap_lag">
         <input> fcs/flap </input>
-        <c1> 54 </c1>
+        <c1> 50.0 </c1>
         <output> fcs/flap_lag</output>
       </lag_filter>
 
@@ -321,7 +321,7 @@
         <function>
                 <product>
                         <property>fcs/front_motor_lag</property>
-                        <value>7.643508703</value>
+                        <value>6.36958</value>
                 </product>
         </function>
         <location unit="IN">
@@ -340,7 +340,7 @@
         <function>
                 <product>
                         <property>fcs/right_motor_lag</property>
-                        <value>7.643508703</value>
+                        <value>6.36958</value>
                 </product>
         </function>
         <location unit="IN">
@@ -359,7 +359,7 @@
         <function>
                 <product>
                         <property>fcs/back_motor_lag</property>
-                        <value>7.643508703</value>
+                        <value>6.36958</value>
                 </product>
         </function>
         <location unit="IN">
@@ -378,7 +378,7 @@
         <function>
                 <product>
                         <property>fcs/left_motor_lag</property>
-                        <value>7.643508703</value>
+                        <value>6.36958</value>
                 </product>
         </function>
         <location unit="IN">
@@ -825,7 +825,7 @@
         <product>
           <property>aero/qbar-psf</property>
           <value>47.9</value> <!-- Conversion to pascals -->
-          <value>0.0453</value> <!-- CD x Area (m^2) -->
+          <value>0.108</value> <!-- CD x Area (m^2) -->
           <value>0.224808943</value> <!-- N to LBS -->
         </product>
       </function>
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index 53db29d588..8d759b4332 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -544,7 +544,7 @@
    airframe="airframes/tudelft/rotwing_v3b.xml"
    radio="radios/crossfire_sbus.xml"
    telemetry="telemetry/highspeed_rotorcraft.xml"
-   flight_plan="flight_plans/tudelft/rotwing_EHVB.xml"
+   flight_plan="flight_plans/tudelft/rotwing_generic.xml"
    settings="settings/rotorcraft_basic.xml"
    settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rotwing.xml modules/ekf_aw.xml modules/electrical.xml modules/follow_me.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi_hybrid.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/imu_heater.xml modules/ins_ekf2.xml modules/lidar_tfmini.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
    gui_color="red"
diff --git a/sw/airborne/modules/nav/ground_detect.c b/sw/airborne/modules/nav/ground_detect.c
index 1b4c9aa714..a3d4ad7431 100644
--- a/sw/airborne/modules/nav/ground_detect.c
+++ b/sw/airborne/modules/nav/ground_detect.c
@@ -40,6 +40,10 @@
 #endif
 #endif
 
+#ifndef GROUND_DETECT_SPECIFIC_THRUST_THRESHOLD
+#define GROUND_DETECT_SPECIFIC_THRUST_THRESHOLD -5.0
+#endif
+
 #include "pprzlink/messages.h"
 #include "modules/datalink/downlink.h"
 
@@ -103,7 +107,7 @@ void ground_detect_periodic()
 
   // Detect ground based on AND of all triggers
   if ((fabsf(vspeed_ned) < 5.0)
-      && (spec_thrust_down > -5.0)
+      && (spec_thrust_down > GROUND_DETECT_SPECIFIC_THRUST_THRESHOLD)
       && (fabsf(accel_filter.o[0]) < 2.0)
 #if USE_GROUND_DETECT_AGL_DIST
       && (agl_dist_valid && (agl_dist_value_filtered < GROUND_DETECT_AGL_MIN_VALUE))
diff --git a/sw/airborne/modules/nav/waypoints.c b/sw/airborne/modules/nav/waypoints.c
index 862dde09c4..8bd7073178 100644
--- a/sw/airborne/modules/nav/waypoints.c
+++ b/sw/airborne/modules/nav/waypoints.c
@@ -33,11 +33,16 @@ struct Waypoint waypoints[NB_WAYPOINT];
 
 #if PERIODIC_TELEMETRY
 #include "modules/datalink/telemetry.h"
+#include "math/pprz_random.h"
 
 static void send_wp_moved(struct transport_tx *trans, struct link_device *dev)
 {
   static uint8_t i;
   i++;
+
+  // Randomness added for multiple transport devices
+  if (rand_uniform() > 0.02) { i++; }
+
   if (i >= nb_waypoint) { i = 0; }
   pprz_msg_send_WP_MOVED_ENU(trans, dev, AC_ID,
                              &i,
diff --git a/sw/ground_segment/python/rotwing_mon/rotwing_viewer.py b/sw/ground_segment/python/rotwing_mon/rotwing_viewer.py
index 7dc98527cc..77fe5cff45 100644
--- a/sw/ground_segment/python/rotwing_mon/rotwing_viewer.py
+++ b/sw/ground_segment/python/rotwing_mon/rotwing_viewer.py
@@ -25,7 +25,6 @@ import math
 import datetime
 import numpy as np
 import pyttsx3
-import re
 engine = pyttsx3.init()
 
 PPRZ_HOME = os.getenv("PAPARAZZI_HOME", os.path.normpath(os.path.join(os.path.dirname(os.path.abspath(__file__)),