diff --git a/conf/airframes/tudelft/rot_wing_25kg.xml b/conf/airframes/tudelft/rot_wing_25kg.xml
index e89b3d7680..e8e6048f4b 100644
--- a/conf/airframes/tudelft/rot_wing_25kg.xml
+++ b/conf/airframes/tudelft/rot_wing_25kg.xml
@@ -54,13 +54,14 @@
             <define name="RADIO_FBW_MODE"         value="0"/> <!-- Switch between AP and FBW control -->
             <define name="RADIO_KILL_SWITCH"      value="0"/>
             <define name="RADIO_CONTROL_THRUST_X" value="0"/>
+            <define name="RADIO_PARACHUTE"        value="0"/>
 
             <define name="PREFLIGHT_CHECK_BYPASS" value="TRUE"/>
         </target>
 
         <!-- Herelink datalink -->
         <module name="telemetry" type="transparent">
-            <configure name="MODEM_PORT" value="usb_serial"/>
+            <!--configure name="MODEM_PORT" value="usb_serial"/-->
             <configure name="MODEM_BAUD" value="B460800"/>
         </module>
 
@@ -81,29 +82,34 @@
             <define name="AIRSPEED_UAVCAN_SEND_ABI"         value="0" /> <!-- Read Airspeed for logging but do not use it -->
         </module>
         <module name="air_data"/>
+
+        <configure name="PRIMARY_GPS" value="ublox"/>
+        <configure name="SECONDARY_GPS" value="ublox2"/>
         <module name="gps" type="ublox">
-            <configure name="UBX_GPS_BAUD"  value="B460800"/>
+            <configure name="UBX_GPS_BAUD" value="B460800"/>
+            <configure name="UBX2_GPS_BAUD" value="B460800"/>
             <define name="USE_GPS_UBX_RTCM" value="TRUE"/>
         </module>
 
         <!-- IMU / INS -->
         <module name="imu" type="pixhawk6x"/>
         <module name="ins" type="ekf2">
-            <!-- The Cube is mounted 25cm backwards of the CG -->
-            <define name="INS_EKF2_IMU_POS_X" value="-0.25"/>
+            <!-- The Cube is mounted 37cm forwards of the CG -->
+            <define name="INS_EKF2_IMU_POS_X" value="0.37"/>
             <define name="INS_EKF2_IMU_POS_Y" value="0.0"/>
             <define name="INS_EKF2_IMU_POS_Z" value="0.0"/>
 
-            <!-- The main GPS is mounted at the tail 1.44m backwards and 20cm above the CG -->
-            <define name="INS_EKF2_GPS_POS_X" value="-1.44"/>
+            <!-- The main GPS is mounted at the tail 1.44m backwards and 10cm above the CG -->
+            <define name="INS_EKF2_GPS_POS_X" value="-1.28"/>
             <define name="INS_EKF2_GPS_POS_Y" value="0.0"/>
-            <define name="INS_EKF2_GPS_POS_Z" value="0.2"/>
+            <define name="INS_EKF2_GPS_POS_Z" value="0.1"/>
         </module>
 
         <!-- Actuators on dual CAN bus -->
         <module name="actuators" type="uavcan">
             <configure value="TRUE" name="UAVCAN_USE_CAN1"/>
             <configure value="TRUE" name="UAVCAN_USE_CAN2"/>
+            <define value="FALSE" name="UAVCAN_ACTUATORS_USE_CURRENT"/>
         </module>
         <!-- Rotation mechanism actuator on UART -->
         <module name="actuators" type="faulhaber"/>
@@ -142,42 +148,42 @@
 
     <!-- Can bus 1 actuators -->
     <servos driver="Uavcan1">
-        <servo no="0"  name="MOTOR_FRONT"     min="-3276" neutral="-2437" max="3605"/> <!-- Supercan (1100-1202.56-1940us) -->
-        <servo no="1"  name="MOTOR_RIGHT"     min="-3276" neutral="-2437" max="3605"/> <!-- Supercan (1100-1202.56-1940us) -->
-        <servo no="2"  name="MOTOR_BACK"      min="-3276" neutral="-2437" max="3605"/> <!-- Supercan (1100-1202.56-1940us) -->
-        <servo no="3"  name="MOTOR_LEFT"      min="0"     neutral="1000"  max="8191"/> <!-- T-Motor ESC  is neutral -->
-        <servo no="4"  name="MOTOR_PUSH"      min="-3276" neutral="-3276" max="3605"/> <!-- Supercan (1100-1940us) -->
-        <!--servo no="8"  name="AIL_RIGHT"       min="-8000" neutral="-1000"     max="5000"/>
-        <servo no="9"  name="FLAP_RIGHT"      min="-8000" neutral="-1000"     max="5000"/>
-        <servo no="10" name="AIL_LEFT"        min="-5000" neutral="1000"     max="8000"/>
-        <servo no="11" name="FLAP_LEFT"       min="-5000" neutral="1000"     max="8000"/>
-        <servo no="12" name="PARACHUTE"       min="-8191" neutral="0"     max="8191"/-->
+        <servo no="0"  name="BMOTOR_FRONT"     min="0"     neutral="1000"  max="8191"/> <!-- Supercan (1100-1202.56-1940us) -->
+        <servo no="1"  name="BMOTOR_RIGHT"     min="0"     neutral="1000"  max="8191"/> <!-- Supercan (1100-1202.56-1940us) -->
+        <servo no="2"  name="BMOTOR_BACK"      min="0"     neutral="1000"  max="8191"/> <!-- Supercan (1100-1202.56-1940us) -->
+        <servo no="3"  name="MOTOR_LEFT"       min="0"     neutral="1000"  max="8191"/> <!-- T-Motor ESC  is neutral -->
+        <servo no="4"  name="BMOTOR_PUSH"      min="-3276" neutral="-3276" max="3605"/> <!-- Supercan (1100-1940us) -->
+        <servo no="5"  name="BPARACHUTE"       min="-8191" neutral="0"     max="8191"/>
     </servos>
 
     <!-- CAN BUS 1 command outputs-->
     <servos driver="Uavcan1Cmd">
-        <servo no="5"  name="SERVO_ELEVATOR"   min="-3250" neutral="0" max="3250"/>
-        <servo no="6"  name="SERVO_RUDDER"  min="-3250" neutral="0" max="3250"/>
+        <servo no="6"  name="SERVO_ELEVATOR"   min="-7300" neutral="-7300" max="4650"/> <!-- 2500 is level 4650 is 10deg -7300 is -50deg -->
+        <servo no="7"  name="SERVO_RUDDER"     min="-4000" neutral="0" max="4000"/>
+        <servo no="8"  name="AIL_RIGHT"        min="-3250" neutral="0" max="3250"/>
+        <servo no="9"  name="FLAP_RIGHT"       min="-3250" neutral="0" max="3250"/>
+        <servo no="10" name="AIL_LEFT"         min="-3250" neutral="0" max="3250"/>
+        <servo no="11" name="FLAP_LEFT"        min="-3250" neutral="0" max="3250"/>
     </servos>
 
     <!-- Can bus 2 actuators -->
     <servos driver="Uavcan2">
-        <servo no="0"  name="BMOTOR_FRONT"    min="0"     neutral="1000"  max="8191"/> <!-- T-Motor ESC -->
-        <servo no="1"  name="BMOTOR_RIGHT"    min="0"     neutral="1000"  max="8191"/> <!-- T-Motor ESC -->
-        <servo no="2"  name="BMOTOR_BACK"     min="0"     neutral="1000"  max="8191"/> <!-- T-Motor ESC -->
-        <servo no="3"  name="BMOTOR_LEFT"     min="-3276" neutral="-2437" max="3605"/> <!-- Supercan (1100-1202.56-1940us) -->
-        <servo no="4"  name="BMOTOR_PUSH"     min="-3276" neutral="-3276" max="3605"/> <!-- Supercan (1100-1940us) -->
-        <!--servo no="8"  name="BAIL_RIGHT"      min="-8000" neutral="-1000"     max="5000"/>
-        <servo no="9"  name="BFLAP_RIGHT"     min="-8000" neutral="-1000"     max="5000"/>
-        <servo no="10" name="BAIL_LEFT"       min="-5000" neutral="1000"     max="8000"/>
-        <servo no="11" name="BFLAP_LEFT"      min="-5000" neutral="1000"     max="8000"/>
-        <servo no="12" name="BPARACHUTE"      min="-8191" neutral="0"     max="8191"/-->
+        <servo no="0"  name="MOTOR_FRONT"    min="0"     neutral="1000"  max="8191"/> <!-- T-Motor ESC -->
+        <servo no="1"  name="MOTOR_RIGHT"    min="0"     neutral="1000"  max="8191"/> <!-- T-Motor ESC -->
+        <servo no="2"  name="MOTOR_BACK"     min="0"     neutral="1000"  max="8191"/> <!-- T-Motor ESC -->
+        <servo no="3"  name="BMOTOR_LEFT"    min="0"     neutral="1000"  max="8191"/> <!-- Supercan (1100-1202.56-1940us) -->
+        <servo no="4"  name="MOTOR_PUSH"     min="-3276" neutral="-3276" max="3605"/> <!-- Supercan (1100-1940us) -->
+        <servo no="5"  name="PARACHUTE"      min="-8191" neutral="0"     max="8191"/>
     </servos>
 
     <!-- CAN BUS 1 command outputs-->
     <servos driver="Uavcan2Cmd">
-        <servo no="5"  name="BSERVO_ELEVATOR"   min="-3250" neutral="0" max="3250"/>
-        <servo no="6"  name="BSERVO_RUDDER"  min="-3250" neutral="0" max="3250"/>
+        <servo no="6"  name="BSERVO_ELEVATOR"   min="-7300" neutral="-7300" max="4650"/> <!-- 2500 is level 4650 is 10deg -7300 is -50deg -->
+        <servo no="7"  name="BSERVO_RUDDER"     min="-4000" neutral="0" max="4000"/>
+        <servo no="8"  name="BAIL_RIGHT"        min="-3250" neutral="0" max="3250"/>
+        <servo no="9"  name="BFLAP_RIGHT"       min="-3250" neutral="0" max="3250"/>
+        <servo no="10" name="BAIL_LEFT"         min="-3250" neutral="0" max="3250"/>
+        <servo no="11" name="BFLAP_LEFT"        min="-3250" neutral="0" max="3250"/>
     </servos>
 
     <servos driver="Faulhaber">
@@ -194,9 +200,10 @@
 
 
     <command_laws>
-        <let var="th_hold" value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
-        <let VAR="servo_hold" VALUE="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
-        <let VAR="hover_off" VALUE="Or($th_hold, bool_disable_hover_motors)"/>
+        <let var="rc_motors_off"    value="Or(LessThan(RadioControlValues(RADIO_KILL_SWITCH), -4800), MoreThan(RadioControlValues(RADIO_PARACHUTE), 4800))"/>
+        <let var="th_hold"          value="Or($rc_motors_off, !autopilot_get_motors_on())"/>
+        <let var="servo_hold"       value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
+        <let var="hover_off"        value="Or($th_hold, bool_disable_hover_motors)"/>
 
         <call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>
         <!-- Main bus -->
@@ -211,7 +218,7 @@
         <set servo="FLAP_RIGHT"         value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/>
         <set servo="AIL_LEFT"           value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
         <set servo="FLAP_LEFT"          value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/>
-        <set servo="PARACHUTE"          value="RadioControlValues(RADIO_AUX6)"/-->
+        <set servo="PARACHUTE"          value="RadioControlValues(RADIO_PARACHUTE)"/>
 
         <!-- Second bus -->
         <set servo="BMOTOR_FRONT"       value="($hover_off? -9600 : actuators_pprz[0])"/>
@@ -225,7 +232,7 @@
         <set servo="BFLAP_RIGHT"        value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/>
         <set servo="BAIL_LEFT"          value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[6])"/>
         <set servo="BFLAP_LEFT"         value="($servo_hold? RadioControlValues(RADIO_ROLL) : actuators_pprz[7])"/>
-        <set servo="BPARACHUTE"         value="RadioControlValues(RADIO_AUX6)"/-->
+        <set servo="BPARACHUTE"         value="RadioControlValues(RADIO_PARACHUTE)"/>
 
         <!-- Rotation mechanism -->
         <set servo="ROTATION_MECH"      value="rotwing_state_skewing.servo_pprz_cmd"/>
@@ -238,7 +245,7 @@
 
         <define name="AUTO_DOUBLETS_N_ACTUATORS" value="4"/>
         <define name="AUTO_DOUBLETS_ACTUATORS" value="{0,1,2,3}"/>
-        <define name="AUTO_DOUBLETS_AMPLITUDE" value="{1500,1500,1500,1500}"/>
+        <define name="AUTO_DOUBLETS_AMPLITUDE" value="{1500,1000,1500,1000}"/>
 
         <define name="AUTO_DOUBLETS_TIME" value="1.0"/>
         <define name="AUTO_DOUBLETS_INTERVAL" value="3.0"/>
@@ -249,22 +256,22 @@
     </section>
 
     <section name="CTRL_EFF_SHED" prefix="ROT_WING_EFF_SCHED_">
-        <define name="IXX_BODY"              value="0.2845"/>
-        <define name="IYY_BODY"              value="5.078"/>
-        <define name="IZZ"                   value="7.051"/>
-        <define name="IXX_WING"              value="0.712"/>
-        <define name="IYY_WING"              value="1.77"/>
-        <define name="M"                     value="25.5"/>
+        <define name="IXX_BODY"              value="0.3953"/>
+        <define name="IYY_BODY"              value="8.472"/>
+        <define name="IZZ"                   value="10.18"/>
+        <define name="IXX_WING"              value="0.5385"/>
+        <define name="IYY_WING"              value="1.671"/>
+        <define name="M"                     value="23.66"/>
 
-        <define name="DM_DPPRZ_HOVER_PITCH"  value="0.00925"/>
-        <define name="DM_DPPRZ_HOVER_ROLL"   value="{0.0085, 0}"/>
+        <define name="DM_DPPRZ_HOVER_PITCH"  value="0.01326745"/>
+        <define name="DM_DPPRZ_HOVER_ROLL"   value="{77.768,0.0}"/>
 
-        <define name="HOVER_ROLL_PITCH_COEF" value="{0.015,     -0.011}"/>
+        <define name="HOVER_ROLL_PITCH_COEF" value="{0.01994434, -0.00885004}"/>
         <define name="HOVER_ROLL_ROLL_COEF"  value="{0.0,       -0.0}"/>
 
         <define name="K_ELEVATOR"            value="{ 1.27655, -13.3525, -96.0}"/>
         <define name="K_RUDDER"              value="{-72.5037,  -0.9329, -3.23651}"/>
-        <define name="K_AILERON"             value="2.777647188"/>
+        <define name="K_AILERON"             value="5.0"/>
         <define name="K_FLAPERON"            value="2.0439"/>
         <define name="K_PUSHER"              value="{0.027775,-2.41146}"/>
 
@@ -290,13 +297,13 @@
         <define name="NAV_CLIMB_VSPEED"                 value="2.0" />
         <define name="NAV_DESCEND_VSPEED"               value="-1.0"/>
         <define name="NAV_CARROT_DIST"                  value="200"/>
-        <!-- prevent in-flight in start engine block-->
-        <define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST" value="4000"/>
+        <define name="AUTOPILOT_IN_FLIGHT_MIN_THRUST"   value="300"/>
         <define name="ARRIVED_AT_WAYPOINT"              value="50.0"/>
         <define name="NO_GPS_LOST_WITH_DATALINK_TIME"   value="20"/>
         <define name="NO_GPS_LOST_WITH_RC_VALID"        value="TRUE"/>
         <define name="USE_AIRSPEED"                     value="TRUE"/>
         <define name="NAV_HYBRID_MAX_DECELERATION"      value="0.5"/>
+        <define name="MULTI_GPS_MODE"                   value="GPS_MODE_PRIMARY"/>
     </section>
 
     <section name="GROUND_DETECT">
@@ -310,9 +317,53 @@
     </section>
 
     <section name="IMU" prefix="IMU_">
-        <define name="ACCEL_CALIB" value="{{.abi_id=20, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-3,3,13}, .scale={{15146,60067,2567},{1551,6124,263}}, .filter_sample_freq=1138.7, .filter_freq=30}, {.abi_id=22, .calibrated={.neutral=true, .scale=true, .filter=true},.neutral={-26,9,2}, .scale={{32863,18346,55169},{6721,3745,11294}}, .filter_sample_freq=1139.3, .filter_freq=30}}"/>
-        <define name="MAG_CALIB"   value="{{.abi_id=5,  .calibrated={.neutral=true, .scale=true},.neutral={-96,-95,-292}, .scale={{33136,8709,8764},{59233,15343,15761}}}}"/> <!-- Calibrated 't Harde 17-08-2023 -->
-        <define name="GYRO_CALIB"  value="{{.abi_id=20, .calibrated={.filter=true}, .filter_sample_freq=1138.7, .filter_freq=30}, {.abi_id=22, .calibrated={.filter=true}, .filter_sample_freq=1139.3, .filter_freq=30}}"/>
+        <!-- Magnetometers calibration (08-04-2024 @ Valkenburg) -->
+        <define name="MAG_CALIB" type="array">
+            <field type="struct">
+                <field name="abi_id" value="5"/>
+                <field name="calibrated" type="struct">
+                    <field name="neutral" value="true"/>
+                    <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="body_to_sensor" value="{{0,-16384,0, -16384,0,0, 0,0,-16384}}"/>
+            </field>
+        </define>
+
+        <!-- Accelerometers calibration -->
+        <define name="ACCEL_CALIB" type="array">
+            <field type="struct">
+                <field name="abi_id" value="26"/>
+                <field name="calibrated" type="struct">
+                    <field name="neutral" value="false"/>
+                    <field name="scale" value="false"/>
+                    <field name="rotation" value="false"/>
+                    <field name="filter" value="true"/>
+                </field>
+                <field name="filter_sample_freq" value="4042"/>
+                <field name="filter_freq" value="30"/>
+            </field>
+        </define>
+
+        <!-- Gyrometer calibration -->
+        <define name="GYRO_CALIB" type="array">
+            <field type="struct">
+                <field name="abi_id" value="26"/>
+                <field name="calibrated" type="struct">
+                    <field name="neutral" value="false"/>
+                    <field name="scale" value="false"/>
+                    <field name="rotation" value="false"/>
+                    <field name="filter" value="true"/>
+                </field>
+                <field name="filter_sample_freq" value="4042"/>
+                <field name="filter_freq" value="30"/>
+            </field>
+        </define>
+
+        <!-- High speed IMU logging for debugging only -->
+        <!--define name="LOG_HIGHSPEED" value="TRUE"/-->
 
         <!-- Define axis in hover frame -->
         <define name="BODY_TO_IMU_PHI"      value="180." unit="deg"/>
@@ -320,19 +371,13 @@
         <define name="BODY_TO_IMU_PSI"      value="0." unit="deg"/>
     </section>
 
-    <section name="MAG_RM3100" prefix="RM3100_">
-      <define name="MAG_TO_IMU_PHI" value="-90.0"/>
-      <define name="MAG_TO_IMU_THETA" value="0.0"/>
-      <define name="MAG_TO_IMU_PSI" value="0.0"/>
-    </section>
-
     <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
         <!-- Limits -->
         <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"/>
-        <define name="SP_PSI_DELTA_LIMIT"   value="45" unit="deg"/>
+        <define name="SP_PSI_DELTA_LIMIT"   value="5"   unit="deg"/>
 
         <!-- Reference model -->
         <define name="REF_OMEGA_P"          value="800" unit="deg/s"/>
@@ -378,9 +423,9 @@
         <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="{12.1, 12.1, 12.1, 12.1, 52.7,  52.7, 52.7, 52.7, 24.1}"/>
-        <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}" />
+        <define name="ACT_FREQ"             value="{13.68, 13.68, 13.68, 13.68, 52.7,  52.7, 52.7, 52.7, 24.1}"/>
+        <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}" />
 
         <define VALUE="{1000, 1000, 1, 100, 100}" NAME="WLS_PRIORITIES"/>
         <define VALUE="{1.3, 1.3, 1.3, 1.3, 1.0, 1.0, 1.0, 1.0, 1.0}" NAME="WLS_WU"/>
@@ -400,9 +445,9 @@
         <define name="MAX_R"                        value="30.0" unit="deg/s"/>
 
         <!-- Filters -->
-        <define name="FILTER_RATES_SECOND_ORDER"        value="TRUE" />
-        <define name="FILT_CUTOFF_P"                    value="10.0"/>
-        <define name="FILT_CUTOFF_Q"                    value="10.0"/>
+        <define name="FILTER_RATES_SECOND_ORDER"        value="FALSE" />
+        <define name="FILT_CUTOFF_P"                    value="5.0"/>
+        <define name="FILT_CUTOFF_Q"                    value="5.0"/>
         <define name="FILT_CUTOFF_R"                    value="5.0"/>
         <define name="FILT_CUTOFF"                      value="2.0"/>
         <define name="ESTIMATION_FILT_CUTOFF"           value="2.0"/>
@@ -456,15 +501,17 @@
         <define name="SPEED_GAINZ"          value="0.6"/>
 
         <!-- Other -->
-        <define name="FILTER_CUTOFF"        value="2.5"/>
+        <define name="FILTER_CUTOFF"        value="2.0"/>
         <define name="HEADING_BANK_GAIN"    value="5."/>
-        <define name="MAX_AIRSPEED"         value="19.0"/>
+        <define name="MAX_AIRSPEED"         value="25.0"/>
         <define name="PITCH_LIFT_EFF"       value="0.0"/>
-        <define name="ZERO_AIRSPEED"        value="TRUE"/>
+        <define name="ZERO_AIRSPEED"        value="FALSE"/>
 
         <define name="THRUST_Z_EFF"         value="-0.0023"/>
         <define name="THRUST_X_EFF"         value="0.00055"/>
 
+        <define name="MAX_PUSHER_INCREMENT" value="2000"/>
+
         <define name="NAV_CIRCLE_DIST"      value="60."/>
         <define name="NAV_LINE_DIST"        value="100"/>
         <define name="LINE_GAIN"            value="0.2"/>
@@ -523,7 +570,9 @@
         <item name="goal" type="text">Goal of the flight</item>
         <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="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>
diff --git a/conf/flight_plans/tudelft/rotating_wing_EHVB.xml b/conf/flight_plans/tudelft/rotating_wing_EHVB.xml
index f667da59ba..c5e36c4c1c 100644
--- a/conf/flight_plans/tudelft/rotating_wing_EHVB.xml
+++ b/conf/flight_plans/tudelft/rotating_wing_EHVB.xml
@@ -9,10 +9,10 @@
     <waypoint name="begin_trans" x="300." y="90."/>
     <waypoint name="end_trans" x="300." y="120."/>
     <waypoint name="STDBY" x="60" y="20"/>
-    <!-- <waypoint name="p1" x="65" y="75"/>
+    <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="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"/>
@@ -92,6 +92,8 @@
         <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">
@@ -112,6 +114,26 @@
         <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_FREE)"/>
         <stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
     </block>
+    <block name="stay_p1">
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
+        <stay wp="p1"/>
+    </block>
+    <block name="stay_p2">
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
+        <stay wp="p2"/>
+    </block>
+    <block name="p1_p2">
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
+        <go wp="p1"/>
+        <stay until="stage_time>5" wp="p1"/>
+        <go wp="p2"/>
+        <stay until="stage_time>5" wp="p2"/>
+        <deroute block="p1_p2"/>
+    </block>
+    <!-- <block name="Standby_fwd">
+        <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HYBRID)"/>
+        <stay wp="STDBY" pre_call="rot_wing_vis_transition(WP_trans, WP_decel, WP_end_trans)"/>
+    </block> -->
     <!-- <block name="Approach APP">
       <call_once fun="rotwing_request_configuration(ROTWING_CONFIGURATION_HOVER)"/>
       <stay alt="WaypointAlt(WP_APP)" pre_call="approach_moving_target_enable(WP_APP)" wp="APP"/>
diff --git a/conf/modules/actuators_faulhaber.xml b/conf/modules/actuators_faulhaber.xml
index c3698b129a..e200db7e3e 100644
--- a/conf/modules/actuators_faulhaber.xml
+++ b/conf/modules/actuators_faulhaber.xml
@@ -4,6 +4,13 @@
   <doc>
     <description>
       Actuators Driver for the Faulhaber controller
+      Currently initialization is done manually.
+      Set mode to:
+       - 2
+       - 4
+       - 2 (it will home slowly)
+       - Wait until homing is finished!
+       - Back to 1 and the wing will move
     </description>
     <configure name="FAULHABER_DEV" value="UARTX" description="UART port (default UART4)"/>
   </doc>
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index c3e988a98c..c7957c7557 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -546,7 +546,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/rotating_wing_EHVB.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_rot_wing.xml modules/electrical.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/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
+   settings_modules="modules/actuators_faulhaber.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.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/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/rot_wing_automation.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"
   />
   <aircraft
@@ -557,7 +557,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/rotating_wing_EHVB.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_rot_wing.xml modules/ekf_aw.xml modules/electrical.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/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.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/rot_wing_automation.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"
   />
   <aircraft
@@ -568,7 +568,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/rotating_wing_EHVB.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_rot_wing.xml modules/ekf_aw.xml modules/electrical.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/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.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/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.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"
   />
   <aircraft
@@ -579,7 +579,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/rotating_wing_EHVB.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_rot_wing.xml modules/ekf_aw.xml modules/electrical.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/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.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/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.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"
   />
   <aircraft
@@ -590,7 +590,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/rotating_wing_EHVB.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_rot_wing.xml modules/ekf_aw.xml modules/electrical.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/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.xml modules/rotwing_state.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_doublet.xml modules/target_pos.xml"
+   settings_modules="modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/airspeed_uavcan.xml modules/approach_moving_target.xml modules/eff_scheduling_rot_wing.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/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/parachute.xml modules/pfc_actuators.xml modules/preflight_checks.xml modules/rot_wing_automation.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"
   />
   <aircraft
diff --git a/conf/userconf/tudelft/control_panel.xml b/conf/userconf/tudelft/control_panel.xml
index 8e7a2a12dc..1b08b12dfd 100644
--- a/conf/userconf/tudelft/control_panel.xml
+++ b/conf/userconf/tudelft/control_panel.xml
@@ -245,11 +245,8 @@
         <arg flag="-udp"/>
       </program>
       <program name="Server"/>
-      <program name="GCS">
-        <arg flag="-speech"/>
-        <arg flag="-layout" constant="bottom_settings.xml"/>
-      </program>
       <program name="Messages"/>
+      <program name="PprzGCS"/>
     </session>
     <session name="Flight UDP Optitrack + Joystick">
       <program name="Data Link">
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
index e68ae9dc97..0d38d8c57e 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -58,6 +58,10 @@
 #define GUIDANCE_INDI_LIFTD_ASQ 0.20
 #endif
 
+#ifndef GUIDANCE_INDI_MAX_PUSHER_INCREMENT
+#define GUIDANCE_INDI_MAX_PUSHER_INCREMENT MAX_PPRZ
+#endif
+
 /* If lift effectiveness at low airspeed not defined,
  * just make one interpolation segment that connects to
  * the quadratic part from 12 m/s onward
@@ -554,6 +558,9 @@ struct StabilizationSetpoint guidance_indi_run(struct FloatVect3 *accel_sp, floa
   float thrust_vect[3];
 #if GUIDANCE_INDI_HYBRID_U > 3
   thrust_vect[0] = du_gih[3];
+  if (thrust_vect[0] > MAX_PUSHER_INCREMENT*guidance_indi_thrust_x_eff) {
+    thrust_vect[0] = GUIDANCE_INDI_MAX_PUSHER_INCREMENT*guidance_indi_thrust_x_eff;
+  }
 #else
   thrust_vect[0] = 0;
 #endif
diff --git a/sw/airborne/modules/actuators/actuators_faulhaber.c b/sw/airborne/modules/actuators/actuators_faulhaber.c
index 507f18aa12..d68a2494bf 100644
--- a/sw/airborne/modules/actuators/actuators_faulhaber.c
+++ b/sw/airborne/modules/actuators/actuators_faulhaber.c
@@ -313,7 +313,7 @@ static void faulhaber_parse_msg(struct faulhaber_parser_t *p)
     faulhaber.real_position = p->data[3] | (p->data[4] << 8) | (p->data[5] << 16) | (p->data[6] << 24);
 
     struct act_feedback_t feedback = {0};
-    feedback.position = (faulhaber.real_position - get_servo_min_FAULHABER(0)) / (double)(get_servo_max_FAULHABER(
+    feedback.position = ((faulhaber.real_position/ACTUATORS_FAULHABER_COMMAND_SCALE) - get_servo_min_FAULHABER(0)) / (double)(get_servo_max_FAULHABER(
                           0) - get_servo_min_FAULHABER(0)) * M_PI_2; // In radians
     feedback.set.position = true;
     feedback.idx = get_servo_idx_FAULHABER(0);
diff --git a/sw/airborne/modules/actuators/actuators_faulhaber.h b/sw/airborne/modules/actuators/actuators_faulhaber.h
index 28c253a952..34ac87fe07 100644
--- a/sw/airborne/modules/actuators/actuators_faulhaber.h
+++ b/sw/airborne/modules/actuators/actuators_faulhaber.h
@@ -53,13 +53,15 @@ extern void actuators_faulhaber_event(void);
 extern void actuators_faulhaber_SetMode(uint8_t mode);
 
 
+#define ACTUATORS_FAULHABER_COMMAND_SCALE 1000.0f
+
 #if USE_NPS
 #define ActuatorsFaulhaberInit() {}
 #define ActuatorFaulhaberSet(_i, _v) {}
 #define ActuatorsFaulhaberCommit()  {}
 #else
 #define ActuatorsFaulhaberInit() actuators_faulhaber_init()
-#define ActuatorFaulhaberSet(_i, _v) { faulhaber.setpoint_position = ((get_servo_max_FAULHABER(0)-_v) + get_servo_min_FAULHABER(0))*1000.0f; }
+#define ActuatorFaulhaberSet(_i, _v) { faulhaber.setpoint_position = ((get_servo_max_FAULHABER(0)-_v) + get_servo_min_FAULHABER(0))*ACTUATORS_FAULHABER_COMMAND_SCALE; }
 #define ActuatorsFaulhaberCommit()  {}
 #endif
 
diff --git a/sw/airborne/modules/gps/gps.h b/sw/airborne/modules/gps/gps.h
index debc43d590..29f0aaf030 100644
--- a/sw/airborne/modules/gps/gps.h
+++ b/sw/airborne/modules/gps/gps.h
@@ -36,6 +36,7 @@ extern "C" {
 #include "math/pprz_geodetic_float.h"
 
 #include "mcu_periph/sys_time.h"
+#include "generated/airframe.h"
 
 #define GPS_FIX_NONE 0x00     ///< No GPS fix
 #define GPS_FIX_2D   0x02     ///< 2D GPS fix