diff --git a/conf/airframes/tudelft/rot_wing_25kg.xml b/conf/airframes/tudelft/rot_wing_25kg.xml
index c76944658e..398774817c 100644
--- a/conf/airframes/tudelft/rot_wing_25kg.xml
+++ b/conf/airframes/tudelft/rot_wing_25kg.xml
@@ -304,6 +304,8 @@
         <define name="USE_AIRSPEED"                     value="TRUE"/>
         <define name="NAV_HYBRID_MAX_DECELERATION"      value="0.5"/>
         <define name="MULTI_GPS_MODE"                   value="GPS_MODE_PRIMARY"/>
+        <define name="INS_EKF2_FUSION_MODE"             value="(MASK_USE_GPS|MASK_USE_GPSYAW)"/>
+        <define name="INS_EKF2_GPS_YAW_OFFSET"          value="0"/>
     </section>
 
     <section name="GROUND_DETECT">
@@ -326,8 +328,8 @@
                     <field name="scale" value="true"/>
                     <field name="rotation" value="true"/>
                 </field>
-                <field name="neutral" value="95,217,-115" type="int[]"/>
-                <field name="scale" value="{{29417,8274,1853},{49873,15133,3195}}"/>
+                <field name="neutral" value="-66,372,-134" type="int[]"/>
+                <field name="scale" value="{{34618,27035,27547},{57579,48737,47275}}"/>
                 <field name="body_to_sensor" value="{{0,-16384,0, -16384,0,0, 0,0,-16384}}"/>
             </field>
         </define>
@@ -571,12 +573,14 @@
         <item name="basic law">Location, airspace and weather</item>
         <item name="RC Battery">Check the RC battery</item>
         <item name="wings">Check wings secured (and taped)</item>
+        <item name="rotation">Initialize and check wing rotation</item>
         <item name="attitude">Check attitude and heading</item>
         <item name="airspeed">Calibrate airspeed sensor</item>
         <item name="takeoff location">Put UAV on take-off location</item>
         <item name="flight plan">Check flight plan</item>
         <item name="flight block">Switch to correct flight block</item>
         <item name="camera">Switch on camera</item>
+        <item name="parachute">Arm parachute</item>
         <item name="announce">Announce flight to other airspace users</item>
     </checklist>
 </airframe>
diff --git a/conf/airframes/tudelft/rot_wing_v3b.xml b/conf/airframes/tudelft/rot_wing_v3b.xml
index 324748eca3..be10dabece 100644
--- a/conf/airframes/tudelft/rot_wing_v3b.xml
+++ b/conf/airframes/tudelft/rot_wing_v3b.xml
@@ -467,7 +467,7 @@
         <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="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.20"/>
+        <define name="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.18"/>
         <define name="AGL_DIST_MAX_DISTANCE_CHECK"          value="0.25"/>
         <define name="AGL_DIST_FILTER"                      value="0.07"/>
     </section>
diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml
index 240e870ff4..f6e5f0a414 100644
--- a/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml
+++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop.xml
@@ -240,7 +240,7 @@
         <define name="USE_GROUND_DETECT_INDI_THRUST"        value="FALSE"/>
         <define name="USE_GROUND_DETECT_AGL_DIST"           value="TRUE"/>
         <define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE"   value="0.24"/>
-        <define name="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.20"/>
+        <define name="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.18"/>
         <define name="AGL_DIST_MAX_DISTANCE_CHECK"          value="0.25"/>
         <define name="AGL_DIST_FILTER"                      value="0.07"/>
     </section>
diff --git a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml
index e8d7e3ae19..859d9ea913 100644
--- a/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml
+++ b/conf/airframes/tudelft/rot_wing_v3c_oneloop_optitrack_ext_pose.xml
@@ -240,7 +240,7 @@
         <define name="USE_GROUND_DETECT_INDI_THRUST"        value="FALSE"/>
         <define name="USE_GROUND_DETECT_AGL_DIST"           value="TRUE"/>
         <define name="GROUND_DETECT_SENSOR_AGL_MIN_VALUE"   value="0.24"/>
-        <define name="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.20"/>
+        <define name="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.18"/>
         <define name="AGL_DIST_MAX_DISTANCE_CHECK"          value="0.25"/>
         <define name="AGL_DIST_FILTER"                      value="0.07"/>
     </section>
diff --git a/conf/airframes/tudelft/rot_wing_v3d.xml b/conf/airframes/tudelft/rot_wing_v3d.xml
index 5b3f3efaa6..d88f0a6700 100644
--- a/conf/airframes/tudelft/rot_wing_v3d.xml
+++ b/conf/airframes/tudelft/rot_wing_v3d.xml
@@ -455,7 +455,7 @@
         <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="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.20"/>
+        <define name="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.18"/>
         <define name="AGL_DIST_MAX_DISTANCE_CHECK"          value="0.25"/>
         <define name="AGL_DIST_FILTER"                      value="0.07"/>
     </section>
diff --git a/conf/airframes/tudelft/rot_wing_v3e.xml b/conf/airframes/tudelft/rot_wing_v3e.xml
index ff728c35fd..9d582c4fa1 100644
--- a/conf/airframes/tudelft/rot_wing_v3e.xml
+++ b/conf/airframes/tudelft/rot_wing_v3e.xml
@@ -455,7 +455,7 @@
         <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="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.20"/>
+        <define name="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.18"/>
         <define name="AGL_DIST_MAX_DISTANCE_CHECK"          value="0.25"/>
         <define name="AGL_DIST_FILTER"                      value="0.07"/>
     </section>
diff --git a/conf/airframes/tudelft/rot_wing_v3f.xml b/conf/airframes/tudelft/rot_wing_v3f.xml
index bc9f439973..93799faa67 100644
--- a/conf/airframes/tudelft/rot_wing_v3f.xml
+++ b/conf/airframes/tudelft/rot_wing_v3f.xml
@@ -455,7 +455,7 @@
         <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="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.20"/>
+        <define name="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.18"/>
         <define name="AGL_DIST_MAX_DISTANCE_CHECK"          value="0.25"/>
         <define name="AGL_DIST_FILTER"                      value="0.07"/>
     </section>
diff --git a/conf/airframes/tudelft/rot_wing_v3g.xml b/conf/airframes/tudelft/rot_wing_v3g.xml
index 0c7867c0d3..7200db5d17 100644
--- a/conf/airframes/tudelft/rot_wing_v3g.xml
+++ b/conf/airframes/tudelft/rot_wing_v3g.xml
@@ -462,7 +462,7 @@
         <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="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.20"/>
+        <define name="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.18"/>
         <define name="AGL_DIST_MAX_DISTANCE_CHECK"          value="0.25"/>
         <define name="AGL_DIST_FILTER"                      value="0.07"/>
     </section>
diff --git a/conf/airframes/tudelft/rot_wing_v3h.xml b/conf/airframes/tudelft/rot_wing_v3h.xml
index 7577f2fdfc..26243b110f 100644
--- a/conf/airframes/tudelft/rot_wing_v3h.xml
+++ b/conf/airframes/tudelft/rot_wing_v3h.xml
@@ -462,7 +462,7 @@
         <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="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.20"/>
+        <define name="AGL_DIST_MIN_DISTANCE_CHECK"          value="0.18"/>
         <define name="AGL_DIST_MAX_DISTANCE_CHECK"          value="0.25"/>
         <define name="AGL_DIST_FILTER"                      value="0.07"/>
     </section>
diff --git a/conf/telemetry/highspeed_rotorcraft.xml b/conf/telemetry/highspeed_rotorcraft.xml
index 7197499de3..a25ca6910c 100644
--- a/conf/telemetry/highspeed_rotorcraft.xml
+++ b/conf/telemetry/highspeed_rotorcraft.xml
@@ -49,7 +49,6 @@
       <message name="INS_FLOW_INFO"            period="0.1"/>
       <message name="GPS_RTK"                  period="0.1"/>
       <message name="AOA"                      period="0.2"/>
-      <message name="ACTUATORS_RAW"            period="0.23"/>
       <message name="APPROACH_MOVING_TARGET"   period="0.1"/>
       <message name="AIRSPEED_RAW"             period="0.1"/>
       <message name="TARGET_POS_INFO"          period="0.1"/>
@@ -98,6 +97,7 @@
       <message name="BARO_RAW"          period=".1"/>
       <message name="ARDRONE_NAVDATA"   period=".05"/>
       <message name="AIRSPEED_RAW"      period="0.1"/>
+      <message name="ACTUATORS_RAW"     period="0.1"/>
     </mode>
 
     <mode name="scaled_sensors">
@@ -251,11 +251,9 @@
       <message name="HYBRID_GUIDANCE"          period="0.02"/>
       <message name="ESC"                      period="0.02"/>
       <message name="STAB_ATTITUDE"            period="0.002"/>
-      <message name="PPM"                      period="0.05"/>
       <message name="ACTUATORS"                period="0.002"/>
       <message name="GPS_RTK"                  period="0.1"/>
-      <message name="IMU_HEATER"               period="1.0"/>
-      <message name="AIRSPEED_RAW"             period="0.01"/>
+      <message name="IMU_HEATER"               period="6.2"/>
       <message name="EFF_MAT_G"                period="0.002"/>
       <message name="GUIDANCE"                 period="0.002"/>
       <message name="EXTERNAL_POSE_DOWN"       period="0.002"/>
diff --git a/sw/airborne/modules/imu/imu_heater.c b/sw/airborne/modules/imu/imu_heater.c
index d220d4e0a6..7aaa22b5cb 100644
--- a/sw/airborne/modules/imu/imu_heater.c
+++ b/sw/airborne/modules/imu/imu_heater.c
@@ -81,7 +81,7 @@ static struct preflight_check_t imu_heater_pfc;
 
 /** Maximum error in percentile */
 #ifndef IMU_HEATER_MAX_ERROR
-#define IMU_HEATER_MAX_ERROR 0.15f
+#define IMU_HEATER_MAX_ERROR 0.20f
 #endif
 
 /** ABI id from either guro or accel */
@@ -92,8 +92,8 @@ static struct preflight_check_t imu_heater_pfc;
 #endif
 
 static void imu_heater_preflight(struct preflight_result_t *result) {
-  if(imu_heater.meas_temp < ((1.0f-IMU_HEATER_MAX_ERROR)*imu_heater.target_temp) || imu_heater.meas_temp > ((1.0f+IMU_HEATER_MAX_ERROR)*imu_heater.target_temp)) {
-    preflight_error(result, "IMU %d temperature outside limits %.2f < %.2f < %.2f", IMU_HEATER_ABI_ID, ((1.0f-IMU_HEATER_MAX_ERROR)*imu_heater.target_temp), imu_heater.meas_temp, ((1.0f+IMU_HEATER_MAX_ERROR)*imu_heater.target_temp));
+  if(imu_heater.meas_temp < ((1.0f-IMU_HEATER_MAX_ERROR)*imu_heater.target_temp) || imu_heater.meas_temp > ((1.0f+2*IMU_HEATER_MAX_ERROR)*imu_heater.target_temp)) {
+    preflight_error(result, "IMU %d temperature outside limits %.2f < %.2f < %.2f", IMU_HEATER_ABI_ID, ((1.0f-IMU_HEATER_MAX_ERROR)*imu_heater.target_temp), imu_heater.meas_temp, ((1.0f+2*IMU_HEATER_MAX_ERROR)*imu_heater.target_temp));
   } else {
     preflight_success(result, "IMU %d temperature ok", IMU_HEATER_ABI_ID);
   }
diff --git a/sw/airborne/modules/ins/ins_ekf2.cpp b/sw/airborne/modules/ins/ins_ekf2.cpp
index 0dc31ee517..a2aecdc96e 100644
--- a/sw/airborne/modules/ins/ins_ekf2.cpp
+++ b/sw/airborne/modules/ins/ins_ekf2.cpp
@@ -929,11 +929,13 @@ static void gps_cb(uint8_t sender_id __attribute__((unused)),
   gps_msg.yaw_offset = 0;
 #elif defined(INS_EKF2_GPS_YAW_OFFSET)
   if(ISFINITE(gps_relposned.relPosHeading)) {
-    gps_msg.yaw = wrap_pi(RadOfDeg(gps_relposned.relPosHeading));
+    gps_msg.yaw = wrap_pi(RadOfDeg(gps_relposned.relPosHeading - INS_EKF2_GPS_YAW_OFFSET));
   } else {
     gps_msg.yaw = NAN;
   }
-  gps_msg.yaw_offset = INS_EKF2_GPS_YAW_OFFSET;
+
+  // Offset also needs to be substracted from the heading (this is for roll/pitch angle limits)
+  gps_msg.yaw_offset = RadOfDeg(INS_EKF2_GPS_YAW_OFFSET);
 #else
   gps_msg.yaw = NAN;
   gps_msg.yaw_offset = NAN;