diff --git a/conf/airframes/TUDELFT/tudelft_conf.xml b/conf/airframes/TUDELFT/tudelft_conf.xml
index f7ac572d79..128de52644 100644
--- a/conf/airframes/TUDELFT/tudelft_conf.xml
+++ b/conf/airframes/TUDELFT/tudelft_conf.xml
@@ -214,8 +214,8 @@
    airframe="airframes/TUDELFT/tudelft_outback.xml"
    radio="radios/dummy.xml"
    telemetry="telemetry/rotorcraft_with_logger.xml"
-   flight_plan="flight_plans/dummy.xml"
-   settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml"
+   flight_plan="flight_plans/TUDELFT/tudelft_delft_basic.xml"
+   settings="settings/rotorcraft_basic.xml settings/control/stabilization_att_int.xml settings/control/rotorcraft_guidance.xml settings/control/stabilization_rate.xml"
    settings_modules="modules/opa_controller.xml modules/air_data.xml modules/temp_adc.xml modules/logger_sd_spi_direct.xml modules/gps_ubx_ucenter.xml"
    gui_color="#ffffdffac31f"
   />
diff --git a/conf/airframes/TUDELFT/tudelft_outback.xml b/conf/airframes/TUDELFT/tudelft_outback.xml
index dfd8e5651e..7e055fa418 100644
--- a/conf/airframes/TUDELFT/tudelft_outback.xml
+++ b/conf/airframes/TUDELFT/tudelft_outback.xml
@@ -23,6 +23,7 @@
       <module name="imu"           type="mpu6000"/>
       <module name="gps"           type="ublox"/>
       <module name="stabilization" type="int_quat"/>
+       <module name="stabilization" type="rate"/>
       <module name="ahrs"          type="int_cmpl_quat"/>
       <module name="ins"           type="hff"/>
       <module name="guidance"      type="indi"/>
@@ -38,6 +39,8 @@
         <define name="SDLOGGER_DIRECT_CONTROL_SWITCH" value="AUX3"/>
       </module>
       <module name="gps_ubx_ucenter"/>
+      <module name="heli_throttle_curve"/>
+      <define name="ROTORCRAFT_IS_HELI" value="TRUE" />
     </target>
 
     <!-- FBW (Flight by Wire) part of the board -->
@@ -70,23 +73,30 @@
 
   <servos driver="Pwm">
      <servo name="THROTTLE"       no="0" min="1100" neutral="1100" max="1900"/>
-     <servo name="SW_BACK"        no="1" min="1900" neutral="1500" max="1100"/>
+     <servo name="SW_BACK"        no="1" min="1950" neutral="1550" max="1150"/>
      <servo name="SW_LEFTFRONT"   no="2" min="1100" neutral="1500" max="1900"/>
-     <servo name="SW_RIGHTFRONT"  no="3" min="1900" neutral="1500" max="1100"/>
+     <servo name="SW_RIGHTFRONT"  no="3" min="1880" neutral="1480" max="1080"/>
      <servo name="TAIL_LEFT"      no="4" min="1100" neutral="1500" max="1900"/>
      <servo name="TAIL_RIGHT"     no="5" min="1100" neutral="1500" max="1900"/>
-     <servo name="AIL_LEFTFRONT"  no="6" min="1100" neutral="1500" max="1900"/>
-     <servo name="AIL_LEFTBACK"   no="7" min="1100" neutral="1500" max="1900"/>
-     <servo name="AIL_RIGHTFRONT" no="8" min="1100" neutral="1500" max="1900"/>
-     <servo name="AIL_RIGHTBACK"  no="9" min="1100" neutral="1500" max="1900"/>
+     <servo name="AIL_LEFTLOW"    no="6" min="1100" neutral="1475" max="1900"/>
+     <servo name="AIL_RIGHTLOW"   no="7" min="1100" neutral="1520" max="1900"/>
+     <servo name="AIL_LEFTUP"     no="8" min="1100" neutral="1530" max="1900"/>
+     <servo name="AIL_RIGHTUP"    no="9" min="1100" neutral="1565" max="1900"/>
   </servos>
 
+  <section name="RADIO">
+    <define name="RADIO_TH_HOLD" value="RADIO_AUX1"/>
+    <define name="RADIO_FMODE"   value="RADIO_AUX2"/>
+  </section>
+
   <rc_commands>
-    <set command="THRUST" value="@THROTTLE"/>
-    <set command="ROLL"   value="@ROLL"/>
-    <set command="PITCH"  value="@PITCH"/>
-    <set command="YAW"    value="@YAW"/>
-    <set command="FMODE"  value="@AUX2"/>
+    <set command="THRUST"   value="@THROTTLE"/>
+    <set command="ROLL"     value="@ROLL"/>
+    <set command="PITCH"    value="@PITCH"/>
+    <set command="YAW"      value="@YAW"/>
+    <set command="MODE"     value="@MODE"/>
+    <set command="FMODE"    value="@FMODE"/>
+    <set command="TH_HOLD"  value="@TH_HOLD"/>
   </rc_commands>
 
   <commands>
@@ -94,11 +104,13 @@
     <axis name="ROLL"       failsafe_value="0"/>
     <axis name="PITCH"      failsafe_value="0"/>
     <axis name="YAW"        failsafe_value="0"/>
-    <axis name="FMODE"      failsafe_value="0"/>
+    <axis name="FMODE"      failsafe_value="-9600"/>
+    <axis name="MODE"       failsafe_value="-9600"/>
+    <axis name="TH_HOLD"    failsafe_value="-9600"/>
   </commands>
 
-  <section name="MIXING" prefix="SW_MIXING_">
-    <define name="TYPE" value="HR120"/>
+  <section name="MIXING"      prefix="SW_MIXING_">
+    <define name="TYPE"       value="HR120"/>
     <define name="TRIM_ROLL"  value="0"/>
     <define name="TRIM_PITCH" value="0"/>
     <define name="TRIM_COLL"  value="0"/>
@@ -107,26 +119,40 @@
   <command_laws>
     <call fun="throttle_curve_run(autopilot_motors_on, values)"/>
     <call fun="swashplate_mixing_run(values)"/>
-    <set servo="THROTTLE"       value="throttle_curve.throttle"/>
+    <set servo="THROTTLE"       value="(((@TH_HOLD) > 4800)? throttle_curve.throttle :-9600)"/>
     <set servo="SW_BACK"        value="swashplate_mixing.commands[SW_BACK]"/>
     <set servo="SW_LEFTFRONT"   value="swashplate_mixing.commands[SW_LEFTFRONT]"/>
     <set servo="SW_RIGHTFRONT"  value="swashplate_mixing.commands[SW_RIGHTFRONT]"/>
-    <set servo="TAIL_LEFT"      value="@YAW"/>
-    <set servo="TAIL_RIGHT"     value="-@YAW"/>
-    <set servo="AIL_LEFTFRONT"  value="@YAW"/>
-    <set servo="AIL_LEFTBACK"   value="@YAW"/>
-    <set servo="AIL_RIGHTFRONT" value="@YAW"/>
-    <set servo="AIL_RIGHTBACK"  value="@YAW"/>
+    <set servo="TAIL_LEFT"      value="(((@TH_HOLD) > 4800)? (-@YAW - throttle_curve.throttle*0.5) : 0)"/>
+    <set servo="TAIL_RIGHT"     value="(((@TH_HOLD) > 4800)? (-@YAW - throttle_curve.throttle*0.5) : 0)"/>
+    <set servo="AIL_LEFTLOW"    value="((-4800 > @MODE)? (@PITCH - @ROLL) : 0)"/>
+    <set servo="AIL_RIGHTLOW"   value="((-4800 > @MODE)? (-@PITCH - @ROLL) : 0)"/>
+    <set servo="AIL_LEFTUP"     value="((-4800 > @MODE)? (@PITCH - @ROLL) : 0)"/>
+    <set servo="AIL_RIGHTUP"    value="((-4800 > @MODE)? (-@PITCH - @ROLL) : 0)"/>
   </command_laws>
 
+
+<!-- Pitch RODS black laminated props: 52mm top links 65mm bottom links
+
   <heli_curves>
-    <curve throttle="0,4800,7200,8400,9600" collective="-1000,750,2500,4250,6000"/>
-    <curve throttle="9600,7200,9600" collective="-7500,0,7500"/>
+    <curve throttle="0,7000,7000" collective="-7500,-5000,-2500"/>
+    <curve throttle="7500,8500,9600" collective="-7500,-4500,-1500"/>
+    <curve throttle="7500,7500,7500" collective="-7000,500,8000"/>
+  </heli_curves>
+
+-->
+
+<!-- Pitch RODS white symmetric props: 56mm top links 65mm bottom links -->
+
+  <heli_curves>
+    <curve throttle="0,7000,7000" collective="0,2000,4000"/>
+    <curve throttle="5000,7000,9000" collective="0,2000,4000"/>
+    <curve throttle="7500,8500,9600" collective="0,2000,4000"/>
   </heli_curves>
 
   <section name="MISC">
     <define name="NAV_CLIMB_VSPEED" value="2.5"/>
-    <define name="NAV_DESCEND_VSPEED" value="-1.0"/>
+    <define name="NAV_DESCEND_VSPEED" value="-0.7"/>
     <define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
   </section>
 
@@ -145,7 +171,7 @@
 
     <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
     <define name="BODY_TO_IMU_THETA" value="0." unit="deg"/>
-    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
+    <define name="BODY_TO_IMU_PSI"   value="180." unit="deg"/>
 
     <!-- Rotate magneto compared to imu -90 degress -->
     <define name="TO_MAG_PHI"   value="0." unit="deg"/>
@@ -159,6 +185,12 @@
   </section>
 
   <section name="AHRS" prefix="AHRS_">
+    <!--This airframe vibrates a lot, which causes accel measurements in excess of 1g continuously-->
+    <define name="GRAVITY_HEURISTIC_FACTOR" value="0"/>
+    <define name="PROPAGATE_LOW_PASS_RATES" value="TRUE"/>
+    <define name="PROPAGATE_LOW_PASS_RATES_MUL" value="19"/>
+    <define name="PROPAGATE_LOW_PASS_RATES_DIV" value="20"/>
+
     <!-- values used if no GPS fix, on 3D fix is update by geo_mag module -->
     <define name="H_X" value="0.3770441"/>
     <define name="H_Y" value="0.0193986"/>
@@ -177,7 +209,7 @@
     <!-- feedback -->
     <define name="GAIN_P" value="400"/>
     <define name="GAIN_Q" value="400"/>
-    <define name="GAIN_R" value="350"/>
+    <define name="GAIN_R" value="700"/>
 
     <define name="IGAIN_P" value="75"/>
     <define name="IGAIN_Q" value="75"/>
@@ -206,14 +238,14 @@
     <define name="REF_MAX_RDOT" value="RadOfDeg(1800.)"/>
 
     <!-- feedback -->
-    <define name="PHI_PGAIN" value="500"/>
-    <define name="PHI_DGAIN" value="100"/>
+    <define name="PHI_PGAIN" value="600"/>
+    <define name="PHI_DGAIN" value="260"/>
     <define name="PHI_IGAIN" value="0"/>
-    <define name="THETA_PGAIN" value="500"/>
-    <define name="THETA_DGAIN" value="100"/>
+    <define name="THETA_PGAIN" value="600"/>
+    <define name="THETA_DGAIN" value="260"/>
     <define name="THETA_IGAIN" value="0"/>
-    <define name="PSI_PGAIN" value="200"/>
-    <define name="PSI_DGAIN" value="0"/>
+    <define name="PSI_PGAIN" value="500"/>
+    <define name="PSI_DGAIN" value="460"/>
     <define name="PSI_IGAIN" value="0"/>
 
     <!-- feedforward -->
@@ -223,26 +255,26 @@
   </section>
 
   <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
-    <define name="HOVER_KP"    value="150"/>
-    <define name="HOVER_KD"    value="80"/>
-    <define name="HOVER_KI"    value="20"/>
-    <define name="NOMINAL_HOVER_THROTTLE" value="0.5"/>
-    <define name="ADAPT_THROTTLE_ENABLED" value="TRUE"/>
+    <define name="HOVER_KP"    value="75"/>
+    <define name="HOVER_KD"    value="40"/>
+    <define name="HOVER_KI"    value="6"/>
+    <define name="NOMINAL_HOVER_THROTTLE" value="0.42"/>
+    <define name="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
   </section>
 
   <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
     <define name="MAX_BANK" value="35" unit="deg"/>
     <define name="USE_SPEED_REF" value="TRUE"/>
-    <define name="PGAIN" value="50"/>
-    <define name="DGAIN" value="100"/>
-    <define name="AGAIN" value="70"/>
-    <define name="IGAIN" value="20"/>
+    <define name="PGAIN" value="20"/>
+    <define name="DGAIN" value="40"/>
+    <define name="AGAIN" value="30"/>
+    <define name="IGAIN" value="10"/>
   </section>
 
   <section name="AUTOPILOT">
     <define name="MODE_MANUAL" value="AP_MODE_RATE_DIRECT"/>
-    <define name="MODE_AUTO1"  value="AP_MODE_ATTITUDE_DIRECT"/>
-    <define name="MODE_AUTO2"  value="AP_MODE_NAV"/>
+    <define name="MODE_AUTO1"  value="AP_MODE_RATE_DIRECT"/>
+    <define name="MODE_AUTO2"  value="AP_MODE_ATTITUDE_DIRECT"/>
   </section>
 
 </airframe>
diff --git a/conf/flight_plans/TUDELFT/tudelft_delft_basic.xml b/conf/flight_plans/TUDELFT/tudelft_delft_basic.xml
index 023fe9fd36..0629bc9c9f 100644
--- a/conf/flight_plans/TUDELFT/tudelft_delft_basic.xml
+++ b/conf/flight_plans/TUDELFT/tudelft_delft_basic.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE flight_plan SYSTEM "../flight_plan.dtd">
 
-<flight_plan alt="4" ground_alt="0" lat0="51 59 27.6" lon0="4 22 42.0" max_dist_from_home="60" name="Delft Basic" security_height="2">
+<flight_plan alt="5" ground_alt="0" lat0="51 59 27.6" lon0="4 22 42.0" max_dist_from_home="60" name="Delft Basic" security_height="2">
   <header>
 #include "autopilot.h"
 </header>
@@ -12,9 +12,12 @@
     <waypoint name="p2" x="-0.6" y="21.6"/>
     <waypoint name="p3" x="22.2" y="-26.5"/>
     <waypoint name="p4" x="4.9" y="-34.4"/>
-    <waypoint name="CAM" x="14.2" y="-29.4"/>
     <waypoint name="TD" x="5.6" y="-10.9"/>
   </waypoints>
+  <modules>
+    <!-- extra navigation routines -->
+    <module name="nav" type="heli_spinup"/>
+  </modules>
   <blocks>
     <block name="Wait GPS">
       <call fun="NavKillThrottle()"/>
@@ -23,19 +26,30 @@
     <block name="Geo init">
       <while cond="LessThan(NavBlockTime(), 10)"/>
       <call fun="NavSetGroundReferenceHere()"/>
+      <call fun="nav_set_heading_current()"/>
     </block>
     <block name="Holding point">
       <call fun="NavKillThrottle()"/>
-      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+      <manual pitch="0" roll="0" yaw="0" throttle="0" until="FALSE" vmode="throttle"/>
     </block>
     <block name="Start Engine">
+      <call fun="nav_throttle_curve_set(0)" loop="false"/>
+      <call fun="nav_heli_spinup_setup(7, 0.15)" loop="false"/>
       <call fun="NavResurrect()"/>
-      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+      <call fun="nav_heli_spinup_run()"/>
+      <call fun="nav_throttle_curve_set(1)" loop="false"/>
+      <manual pitch="0" roll="0" yaw="0" throttle="0.15" until="stage_time>7" vmode="throttle"/>
+      <call fun="nav_throttle_curve_set(2)" loop="false"/>
+      <manual pitch="0" roll="0" yaw="0" throttle="0.15" until="stage_time>3" vmode="throttle"/>
+    </block>
+    <block name="Hold Attitude">
+      <manual pitch="0" roll="0" yaw="0" throttle="0.15" until="FALSE" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
       <exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
+      <call fun="nav_set_heading_current()"/>
       <call fun="NavSetWaypointHere(WP_CLIMB)"/>
-      <stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
+      <stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
     </block>
     <block name="Standby" strip_button="Standby" strip_icon="home.png">
       <stay wp="STDBY"/>
@@ -49,7 +63,7 @@
     </block>
     <block name="line_p1_p2">
       <go from="p1" hmode="route" wp="p2"/>
-      <stay until="stage_time>10" wp="p2"/>
+      <stay wp="p2" until="stage_time>10"/>
       <go from="p2" hmode="route" wp="p1"/>
       <deroute block="stay_p1"/>
     </block>
@@ -59,16 +73,6 @@
       <go from="p4" hmode="route" wp="p1"/>
       <deroute block="stay_p1"/>
     </block>
-    <block name="test yaw">
-      <go wp="p1"/>
-      <for from="1" to="16" var="i">
-        <heading alt="WaypointAlt(WP_p1)" course="90 * $i" until="stage_time > 3"/>
-      </for>
-      <deroute block="Standby"/>
-    </block>
-    <block name="circle CAM" pre_call="nav_set_heading_towards_waypoint(WP_CAM)">
-      <circle radius="nav_radius" wp="CAM"/>
-    </block>
     <block name="land here" strip_button="Land Here" strip_icon="land-right.png">
       <call fun="NavSetWaypointHere(WP_TD)"/>
     </block>
@@ -82,7 +86,7 @@
       <stay climb="nav_descend_vspeed" vmode="climb" wp="TD"/>
     </block>
     <block name="landed">
-      <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
+      <attitude pitch="0" roll="0" throttle="0" vmode="throttle" until="FALSE"/>
     </block>
   </blocks>
 </flight_plan>
diff --git a/conf/flight_plans/TUDELFT/tudelft_delft_outback.xml b/conf/flight_plans/TUDELFT/tudelft_delft_outback.xml
index a9910c8c1d..8e2138a07d 100644
--- a/conf/flight_plans/TUDELFT/tudelft_delft_outback.xml
+++ b/conf/flight_plans/TUDELFT/tudelft_delft_outback.xml
@@ -44,7 +44,7 @@
     <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
       <exception cond="stateGetPositionEnu_f()->z > 2.0" deroute="Standby"/>
       <call fun="NavSetWaypointHere(WP_CLIMB)"/>
-      <stay climb="nav_climb_vspeed" vmode="climb" wp="CLIMB"/>
+      <stay vmode="climb" climb="nav_climb_vspeed" wp="CLIMB"/>
     </block>
     <block name="Standby" strip_button="Standby" strip_icon="home.png">
       <stay wp="STDBY"/>
@@ -59,7 +59,7 @@
     </block>
     <block name="line_p1_p2">
       <go from="p1" hmode="route" wp="p2"/>
-      <stay until="stage_time>10" wp="p2"/>
+      <stay wp="p2" until="stage_time>10"/>
       <go from="p2" hmode="route" wp="p1"/>
       <deroute block="stay_p1"/>
     </block>
diff --git a/conf/telemetry/rotorcraft_with_logger.xml b/conf/telemetry/rotorcraft_with_logger.xml
index ea59d32d58..b76465809c 100644
--- a/conf/telemetry/rotorcraft_with_logger.xml
+++ b/conf/telemetry/rotorcraft_with_logger.xml
@@ -29,6 +29,7 @@
       <message name="VECTORNAV_INFO"         period="0.5"/>
       <message name="TEMP_ADC"               period="3.0"/>
       <message name="FBW_STATUS"             period="2.0"/>
+      <message name="MOTOR"                  period="0.5"/>
     </mode>
 
     <mode name="ppm">
@@ -151,10 +152,10 @@
     <mode name="default">
       <message name="ROTORCRAFT_STATUS"      period="1.2"/>
       <message name="DL_VALUE"               period="0.5"/>
-      <message name="ALIVE"                  period="2.1"/>
       <message name="IMU_GYRO_SCALED"        period=".002"/>
-      <message name="IMU_ACCEL_SCALED"       period=".002"/>
+      <!--message name="IMU_ACCEL_SCALED"       period=".002"/-->
       <message name="IMU_MAG_SCALED"         period=".1"/>
+      <message name="ROTORCRAFT_CMD"         period=".002"/>
     </mode>
   </process>
 
diff --git a/sw/airborne/modules/nav/nav_heli_spinup.h b/sw/airborne/modules/nav/nav_heli_spinup.h
index bac44a0e6c..6e2667992b 100644
--- a/sw/airborne/modules/nav/nav_heli_spinup.h
+++ b/sw/airborne/modules/nav/nav_heli_spinup.h
@@ -27,6 +27,8 @@
 #define NAV_HELI_SPINUP_H
 
 #include "std.h"
+#include "paparazzi.h"
+#include "modules/helicopter/throttle_curve.h"
 
 struct nav_heli_spinup_t {
   uint16_t duration;        ///< The duration in seconds to reach the final throttle
diff --git a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
index 52e01aab98..ded7b75416 100644
--- a/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
+++ b/sw/airborne/subsystems/ahrs/ahrs_int_cmpl_quat.c
@@ -100,6 +100,17 @@ PRINT_CONFIG_VAR(AHRS_MAG_ZETA)
 #define AHRS_HEADING_UPDATE_GPS_MIN_SPEED 5.0
 #endif
 
+/** Default Rate filter Low pass */
+#ifdef AHRS_PROPAGATE_LOW_PASS_RATES
+#ifndef AHRS_PROPAGATE_LOW_PASS_RATES_MUL
+#define AHRS_PROPAGATE_LOW_PASS_RATES_MUL 2
+#endif
+
+#ifndef AHRS_PROPAGATE_LOW_PASS_RATES_DIV
+#define AHRS_PROPAGATE_LOW_PASS_RATES_DIV 3
+#endif
+#endif
+
 struct AhrsIntCmplQuat ahrs_icq;
 
 static inline void UNUSED ahrs_icq_update_mag_full(struct Int32Vect3 *mag, float dt);
@@ -187,9 +198,9 @@ void ahrs_icq_propagate(struct Int32Rates *gyro, float dt)
 
   /* low pass rate */
 #ifdef AHRS_PROPAGATE_LOW_PASS_RATES
-  RATES_SMUL(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 2);
+  RATES_SMUL(ahrs_icq.imu_rate, ahrs_icq.imu_rate, AHRS_PROPAGATE_LOW_PASS_RATES_MUL);
   RATES_ADD(ahrs_icq.imu_rate, omega);
-  RATES_SDIV(ahrs_icq.imu_rate, ahrs_icq.imu_rate, 3);
+  RATES_SDIV(ahrs_icq.imu_rate, ahrs_icq.imu_rate, AHRS_PROPAGATE_LOW_PASS_RATES_DIV);
 #else
   RATES_COPY(ahrs_icq.imu_rate, omega);
 #endif