From 6435c9fc43e25235d99af022f205c1903b121c79 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 14 Sep 2023 16:58:15 +0200
Subject: [PATCH] [airframes] Airframe fixes (#3061)

---
 conf/airframes/tudelft/nederdrone4.xml        | 253 +++++++-----
 conf/airframes/tudelft/nederdrone5.xml        | 371 ------------------
 conf/airframes/tudelft/nederdrone6.xml        |  12 +-
 conf/airframes/tudelft/nederdrone7.xml        |  10 +-
 conf/airframes/tudelft/nederdrone8.xml        |  21 +-
 .../tudelft/nederdrone_terschelling.xml       |   6 +-
 .../flight_plans/tudelft/nederdrone_troia.xml |  11 +-
 .../tudelft/nederdrone_valkenburg.xml         |   8 +-
 8 files changed, 208 insertions(+), 484 deletions(-)
 delete mode 100644 conf/airframes/tudelft/nederdrone5.xml

diff --git a/conf/airframes/tudelft/nederdrone4.xml b/conf/airframes/tudelft/nederdrone4.xml
index a50cc78880..fa3618dec7 100644
--- a/conf/airframes/tudelft/nederdrone4.xml
+++ b/conf/airframes/tudelft/nederdrone4.xml
@@ -1,6 +1,6 @@
 <!DOCTYPE airframe SYSTEM "../airframe.dtd">
 
-<!-- This is a Nedderdrone
+<!-- This is a Nedderdrone with Trailing edge motors
      * Airframe:    TUD00289
      * Autopilot:   Pixhawk 4
      * Actuators:   12x T-Motor ESC + Motors and 8x Servos (all CAN)
@@ -16,17 +16,23 @@
     <target name="ap" board="px4fmu_5.0_chibios">
       <configure name="PERIODIC_FREQUENCY"  value="500"/>
       <configure name="FLASH_MODE" value="SWD"/>
+      <define name="USE_BARO_BOARD" value="1"/>
+
+      <module name="eff_scheduling_nederdrone">
+        <!-- Trims -->
+        <define name="INDI_SCHEDULING_TRIM_ELEVATOR" value="840"/>
+      </module>
 
       <module name="radio_control" type="sbus">
         <configure name="SBUS_PORT" value="UART3"/>
       </module>
 
-      <module name="airspeed_ets.xml">
-        <configure name="AIRSPEED_ETS_I2C_DEV" value="I2C4"/>
-        <define name="AIRSPEED_ETS_SCALE" value="1.24"/>
+      <module name="airspeed" type="ms45xx_i2c">
+        <define name="USE_I2C4"/>
+        <define name="MS45XX_I2C_DEV" value="i2c4"/>
       </module>
 
-      <module name="scheduling_indi_simple"/>
+
 
       <!-- Forward FuelCell data back to the GCS -->
       <!--module name="generic_uart_sensor"/-->
@@ -37,29 +43,37 @@
       <module name="logger" type="sd_chibios"/>
       <module name="flight_recorder"/>
 
+      <!-- Chibios monitor -->
+      <module name="sys_mon"/>
+
       <define name="ADC_CURRENT_DISABLE" value="TRUE"/>
-      <module name="adc_generic">
-        <configure name="ADC_CHANNEL_GENERIC1" value="ADC_5"/>
-        <configure name="ADC_CHANNEL_GENERIC2" value="ADC_6"/>
-      </module>
 
       <define name="RADIO_TH_HOLD"     value="RADIO_AUX1"/> <!-- Throttle hold in command laws -->
       <define name="RADIO_FMODE"       value="RADIO_AUX2"/> <!-- Throttle curve select -->
       <define name="RADIO_FBW_MODE"    value="RADIO_AUX3"/> <!-- Switch between AP and FBW control -->
       <define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
 
-      <!-- Use the external mag (not in NPS target as then it needs to listen to all) -->
-      <!--define name="INS_EKF2_MAG_ID" value="MAG_LIS3MDL_SENDER_ID"/-->
+      <!-- <module name="ahrs" type="int_cmpl_quat">
+        <configure name="USE_MAGNETOMETER" value="FALSE"/>
+        <define name="AHRS_USE_GPS_HEADING" value="TRUE"/>
+        <define name="AHRS_HEADING_UPDATE_GPS_MIN_SPEED" value="0"/>
+      </module> -->
+
+      <!--module name="ins" type="extended">
+        <define name="INS_USE_GPS_ALT" value="1"/>
+        <define name="INS_USE_GPS_ALT_SPEED" value="1"/>
+        <define name="INS_VFF_R_GPS" value="0.01"/>
+      </module-->
     </target>
 
     <target name="nps" board="pc">
+
+      <module name="eff_scheduling_nederdrone"/>
       <module name="radio_control" type="datalink"/>
       <module name="fdm" type="jsbsim"/>
 
-      <module name="scheduling_indi_simple"/>
-
       <module name="logger_file">
-        <define name="LOGGER_FILE_PATH" value="/home/ewoud/Documents"/>
+        <define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
       </module>
 
       <!--Not dealing with these in the simulation-->
@@ -69,40 +83,61 @@
       <define name="RADIO_KILL_SWITCH" value="0"/>
     </target>
 
+    <!--module name="follow_me">
+      <define name="FOLLOW_ME_DISTANCE" value="60"/>
+      <define name="FOLLOW_ME_HEIGHT" value="40"/>
+    </module-->
+
     <module name="telemetry"     type="transparent">
-      <configure name="MODEM_BAUD" value="B115200"/>
+      <configure name="MODEM_BAUD" value="B460800"/>
     </module>
 
     <module name="approach_moving_target">
       <define name="AMT_ERR_SLOWDOWN_GAIN" value="0.25"/>
     </module>
 
+    <module name="ins"           type="ekf2" />
+
     <module name="actuators"     type="uavcan">
       <configure name="UAVCAN_USE_CAN1" value="TRUE"/>
       <configure name="UAVCAN_USE_CAN2" value="TRUE"/>
     </module>
     <module name="imu"           type="mpu6000"/>
-    <module name="imu"           type="heater"/>
-    <module name="gps"           type="ublox"/>
-    <module name="gps"           type="ubx_ucenter"/>
-    <module name="stabilization" type="indi_simple"/>
+    <!--module name="gps"           type="datalink"/-->
+    <module name="gps"           type="ublox">
+      <configure name="UBX_GPS_BAUD" value="B460800"/>
+      <define name="USE_GPS_UBX_RTCM" value="TRUE"/>
+    </module>
+    <module name="stabilization" type="indi">
+      <configure name="INDI_NUM_ACT" value="8"/>
+      <define name="TILT_TWIST_CTRL" value="TRUE"/>
+    </module>
     <module name="stabilization" type="rate_indi"/>
-    <module name="ins"           type="ekf2" />
 
-    <module name="air_data"/>
+    <module name="air_data" >
+      <define name="AIR_DATA_CALC_AMSL_BARO" value="TRUE" />
+    </module>
+
+    <module name="AOA_pwm">
+      <define name="USE_PWM_INPUT1" value="PWM_PULSE_TYPE_ACTIVE_LOW"/>
+      <define name="AOA_ANGLE_OFFSET" value="3.1415"/>
+      <define name="AOA_PWM_PERIOD" value="1024"/>
+      <define name="AOA_PWM_OFFSET" value="1"/>
+      <configure name="AOA_PWM_CHANNEL" value="PWM_INPUT1" />
+    </module>
 
     <!-- Internal MAG -->
-    <module name="mag_ist8310">
+    <!--module name="mag_ist8310">
       <define name="MODULE_IST8310_UPDATE_AHRS" value="TRUE"/>
       <configure name="MAG_IST8310_I2C_DEV" value="I2C3"/>
-    </module>
+    </module-->
     <!-- External MAG on GPS -->
-    <!--module name="mag_lis3mdl">
+    <module name="mag_lis3mdl">
       <define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
       <configure name="MAG_LIS3MDL_I2C_DEV" value="I2C1"/>
       <define name="LIS3MDL_CHAN_X_SIGN" value="-"/>
       <define name="LIS3MDL_CHAN_Y_SIGN" value="-"/>
-    </module-->
+    </module>
     <!--module name="lidar" type="tfmini">
       <configure name="TFMINI_PORT" value="UART4"/>
       <configure name="USE_TFMINI_AGL" value="FALSE"/>
@@ -114,24 +149,38 @@
 
     <module name="guidance" type="indi_hybrid">
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
-      <define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
-      <define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
-      <define name="GUIDANCE_INDI_POS_GAINZ" value="0.2"/>
-      <define name="GUIDANCE_INDI_SPEED_GAINZ" value="1.0"/>
+      <define name="GUIDANCE_INDI_POS_GAIN" value="0.3"/>
+      <define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>
+      <define name="GUIDANCE_INDI_POS_GAINZ" value="0.3"/>
+      <define name="GUIDANCE_INDI_SPEED_GAINZ" value="0.5"/>
       <define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
       <define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
       <define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
-      <define name="GUIDANCE_INDI_MAX_AIRSPEED" value="16.0"/>
+      <define name="GUIDANCE_INDI_MAX_AIRSPEED" value="17.0"/>
+      <define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/> <!--heading can not be set by navigation-->
       <define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="5"/>
-      <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-943.0"/>
+      <!--define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-943.0"/>
       <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_45" value="-500.0"/>
-      <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_FWD" value="-1600.0"/>
-      <define name="GUIDANCE_INDI_FILTER_CUTOFF" value="0.5"/>
+      <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_FWD" value="-1600.0"/-->
+      <!-- <define name="GUIDANCE_INDI_FILTER_CUTOFF" value="0.5"/> -->
       <define name="GUIDANCE_INDI_LINE_GAIN" value="0.2"/>
-      <define name="GUIDANCE_INDI_MIN_THROTTLE" value="2500"/>
+      <define name="GUIDANCE_INDI_MIN_THROTTLE" value="1500"/>
+      <define name="GUIDANCE_INDI_MIN_THROTTLE_FWD" value="1500"/>
+      <define name="GUIDANCE_INDI_LIFTD_P50" value="6.0"/>
+      <define name="GUIDANCE_INDI_LIFTD_P80" value="10.0"/>
+      <define name="GUIDANCE_INDI_LIFTD_ASQ" value="0.15"/>
     </module>
 
     <module name="motor_mixing"/>
+    <!--module name="sys_id_doublet">
+      <define name="DOUBLET_AXES" value="{0,1,2,3,4,5,6,7}"/>
+      <define name="DOUBLET_RADIO_CHANNEL" value="9"/>
+    </module-->
+    <!--<module name="sys_id_chirp">
+    	<define name="CHIRP_AXES" value="{0,1,2,3,4,5,6,7}"/>
+      <define name="CHIRP_RADIO_CHANNEL" value="9"/>
+    </module>-->
+
   </firmware>
 
   <!-- CAN BUS 1 (Front Wing) -->
@@ -142,10 +191,8 @@
     <servo name="MOTOR_4" no="3" min="-8191" neutral="1500" max="8191"/>
     <servo name="MOTOR_5" no="4" min="-8191" neutral="1500" max="8191"/>
     <servo name="MOTOR_6" no="5" min="-8191" neutral="1500" max="8191"/>
-    <servo name="AIL_1" no="6" min="6000" neutral="0" max="-6000"/>
-    <servo name="FLAP_1" no="7" min="6000" neutral="0" max="-6000"/>
-    <servo name="FLAP_2" no="8" min="-6000" neutral="0" max="6000"/>
-    <servo name="AIL_2" no="9" min="-6000" neutral="0" max="6000"/>
+    <servo name="AIL_1" no="6" min="-6000" neutral="0" max="6000"/>
+    <servo name="AIL_2" no="7" min="6000" neutral="0" max="-6000"/>
   </servos>
 
   <!-- CAN BUS 2 (Back Wing) -->
@@ -156,10 +203,10 @@
     <servo name="MOTOR_10" no="3" min="-8191" neutral="1500" max="8191"/>
     <servo name="MOTOR_11" no="4" min="-8191" neutral="1500" max="8191"/>
     <servo name="MOTOR_12" no="5" min="-8191" neutral="1500" max="8191"/>
-    <servo name="AIL_3" no="6" min="6000" neutral="0" max="-6000"/>
-    <servo name="FLAP_3" no="7" min="6000" neutral="0" max="-6000"/>
-    <servo name="FLAP_4" no="8" min="-6000" neutral="0" max="6000"/>
-    <servo name="AIL_4" no="9" min="-6000" neutral="0" max="6000"/>
+    <servo name="AIL_3" no="6" min="-6000" neutral="0" max="6000"/>
+    <servo name="FLAP_3" no="7" min="-6000" neutral="0" max="6000"/>
+    <servo name="FLAP_4" no="8" min="6000" neutral="0" max="-6000"/>
+    <servo name="AIL_4" no="9" min="6000" neutral="0" max="-6000"/>
   </servos>
 
   <commands>
@@ -181,43 +228,45 @@
     <!-- backleft left (CW), backleft mid (CCW), backleft right (CW), backright left (CCW), backright mid (CW), backright right (CCW) -->
     <define name="NB_MOTOR"    value="12"/>
     <define name="SCALE"       value="256"/>
-    <define name="ROLL_COEF"   value="{256,  157,   56,  -56, -157, -256,  256,  157,   56,  -56, -157, -256}"/>
+    <define name="ROLL_COEF"   value="{256,  253,  159, -159, -253, -256,  256,  157,   56,  -56, -157, -256}"/>
     <define name="PITCH_COEF"  value="{256,  256,  256,  256,  256,  256, -256, -256, -256, -256, -256, -256}"/>
-    <define name="YAW_COEF"    value="{256, -256,  256, -256,  256, -256, -256,  256, -256,  256, -256,  256}"/>
+    <define name="YAW_COEF"    value="{251, -256,  252, -252,  256, -251, -256,  252, -254,  254, -252,  256}"/>
     <!--<define name="YAW_COEF"    value="{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}"/>-->
     <define name="THRUST_COEF" value="{256,  256,  256,  256,  256,  256,  256,  256,  256,  256,  256,  256}"/>
   </section>
 
   <command_laws>
-    <call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
-    <let var="th_hold"     value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
+    <let var="th_hold"     value="Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), !autopilot_get_motors_on())"/>
 
-    <set servo="MOTOR_1"   value="($th_hold? -9600 : motor_mixing.commands[0])"/>
-    <set servo="MOTOR_2"   value="($th_hold? -9600 : motor_mixing.commands[1])"/>
-    <set servo="MOTOR_3"   value="($th_hold? -9600 : motor_mixing.commands[2])"/>
-    <set servo="MOTOR_4"   value="($th_hold? -9600 : motor_mixing.commands[3])"/>
-    <set servo="MOTOR_5"   value="($th_hold? -9600 : motor_mixing.commands[4])"/>
-    <set servo="MOTOR_6"   value="($th_hold? -9600 : motor_mixing.commands[5])"/>
-    <set servo="MOTOR_7"   value="($th_hold? -9600 : motor_mixing.commands[6])"/>
-    <set servo="MOTOR_8"   value="($th_hold? -9600 : motor_mixing.commands[7])"/>
-    <set servo="MOTOR_9"   value="($th_hold? -9600 : motor_mixing.commands[8])"/>
-    <set servo="MOTOR_10"  value="($th_hold? -9600 : motor_mixing.commands[9])"/>
-    <set servo="MOTOR_11"  value="($th_hold? -9600 : motor_mixing.commands[10])"/>
-    <set servo="MOTOR_12"  value="($th_hold? -9600 : motor_mixing.commands[11])"/>
+    <!-- Tip props always at 80% -->
+    <call fun="actuators_pprz[8] = (Or(LessThan(RadioControlValues(RADIO_TH_HOLD), -4800), 0.01 > sched_ratio_tip_props)? -9600 : 9600/100*85*sched_ratio_tip_props);"/>
+    <!--call fun="sys_id_doublet_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/-->
+    <!--<call fun="sys_id_chirp_add_values(autopilot_get_motors_on(),FALSE,actuators_pprz)"/>-->
+
+    <set servo="MOTOR_1"   value="($th_hold? -9600 : actuators_pprz[8])"/>
+    <set servo="MOTOR_2"   value="($th_hold? -9600 : actuators_pprz[0])"/>
+    <set servo="MOTOR_3"   value="($th_hold? -9600 : actuators_pprz[0])"/>
+    <set servo="MOTOR_4"   value="($th_hold? -9600 : actuators_pprz[1])"/>
+    <set servo="MOTOR_5"   value="($th_hold? -9600 : actuators_pprz[1])"/>
+    <set servo="MOTOR_6"   value="($th_hold? -9600 : actuators_pprz[8])"/>
+    <set servo="MOTOR_7"   value="($th_hold? -9600 : actuators_pprz[2])"/>
+    <set servo="MOTOR_8"   value="($th_hold? -9600 : actuators_pprz[2])"/>
+    <set servo="MOTOR_9"   value="($th_hold? -9600 : actuators_pprz[2])"/>
+    <set servo="MOTOR_10"  value="($th_hold? -9600 : actuators_pprz[3])"/>
+    <set servo="MOTOR_11"  value="($th_hold? -9600 : actuators_pprz[3])"/>
+    <set servo="MOTOR_12"  value="($th_hold? -9600 : actuators_pprz[3])"/>
 
     <!-- Removed ApplyDiff for differential control -->
-    <set servo="AIL_1"     value="-2*@PITCH + (  2*@YAW)"/>
-    <set servo="AIL_2"     value="-2*@PITCH + (- 2*@YAW)"/>
-    <set servo="AIL_3"     value=" 2*@PITCH + (  2*@YAW)"/>
-    <set servo="AIL_4"     value=" 2*@PITCH + (- 2*@YAW)"/>
-    <set servo="FLAP_1"    value="-2*@PITCH + (  2*@YAW)"/>
-    <set servo="FLAP_2"    value="-2*@PITCH + (- 2*@YAW)"/>
-    <set servo="FLAP_3"    value=" 2*@PITCH + (  2*@YAW)"/>
-    <set servo="FLAP_4"    value=" 2*@PITCH + (- 2*@YAW)"/>
+    <set servo="AIL_1"     value="($th_hold? 9600 : actuators_pprz[4])"/>
+    <set servo="AIL_2"     value="($th_hold? 9600 : actuators_pprz[5])"/>
+    <set servo="AIL_3"     value="actuators_pprz[6]"/>
+    <set servo="AIL_4"     value="actuators_pprz[7]"/>
+    <set servo="FLAP_3"    value="actuators_pprz[6]"/>
+    <set servo="FLAP_4"    value="actuators_pprz[7]"/>
   </command_laws>
 
   <section name="MISC">
-    <define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 17.9024749557 * adc)"/><!-- TODO: verify/calibrate -->
+    <define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 18.1 * adc)"/><!-- TODO: verify/calibrate -->
     <define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
 
     <!-- Basic navigation settings -->
@@ -225,6 +274,10 @@
     <define name="NAV_DESCEND_VSPEED" value="-0.5"/>
     <define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
 
+    <!-- Settings for circle -->
+    <define name="DEFAULT_CIRCLE_RADIUS" value="700"/>
+    <define name="NAV_CARROT_DIST" value="200"/>
+
     <!-- Avoid GPS loss behavior when having RC or datalink -->
     <define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
     <define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
@@ -253,15 +306,24 @@
     <define name="MPU_Z_SIGN" value="-1"/>
 
     <!-- Calibrated in the MAVLab 14-05-2020 -->
-    <define name="ACCEL_CALIB" value="{{.abi_id=9, .neutral={-423,18,-28}, .scale={{28111,6328,61706},{6151,1291,12775}}}}"/>
+    <define name="ACCEL_X_NEUTRAL" value="-337"/>
+    <define name="ACCEL_Y_NEUTRAL" value="64"/>
+    <define name="ACCEL_Z_NEUTRAL" value="-25"/>
+    <define name="ACCEL_X_SENS" value="4.670307671109528" integer="16"/>
+    <define name="ACCEL_Y_SENS" value="4.9016250738902425" integer="16"/>
+    <define name="ACCEL_Z_SENS" value="4.846689188075245" integer="16"/>
 
     <!-- Calibrated at valkenburg 20-05-2020 (external magnetometer) -->
-    <define name="MAG_CALIB" value="{{.abi_id=4, .neutral={12,113,49}, .scale={{42598,47669,16336},{3039,3365,1153}}}}"/>
-
+    <define name="MAG_X_NEUTRAL" value="866"/>
+    <define name="MAG_Y_NEUTRAL" value="-1530"/>
+    <define name="MAG_Z_NEUTRAL" value="-3313"/>
+    <define name="MAG_X_SENS" value="0.6067461130451115" integer="16"/>
+    <define name="MAG_Y_SENS" value="0.6544292255627779" integer="16"/>
+    <define name="MAG_Z_SENS" value="0.6352539557433349" integer="16"/>
 
     <!-- Define axis in hover frame -->
     <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
-    <define name="BODY_TO_IMU_THETA" value="90." unit="deg"/>
+    <define name="BODY_TO_IMU_THETA" value="82.8" unit="deg"/>
     <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
   </section>
 
@@ -275,16 +337,25 @@
   </section>
 
   <section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
-    <!-- control effectiveness (hover) -->
-    <define name="G1_P" value="0.0030"/>
-    <define name="G1_Q" value="0.0035"/>
-    <define name="G1_R" value="0.0004"/>
-    <define name="G2_R" value="0.00015"/>
+    <!--Nederdrone4 without fuel cell-->
+    <define name="G1_ROLL"   value="{ 0.422, -0.422,  1.14, -1.14,   0.0,   0.0,   0.0,  0.0}"/>
+    <define name="G1_PITCH"  value="{ 0.5,      0.5,  -1.0, - 1.0,  -0.5,  -0.5,  -0.5, -0.5}"/>
+    <define name="G1_YAW"    value="{ 0.0,      0.0,   0.0,   0.0,  0.09, -0.09, -0.09, 0.09}"/>
+    <!-- with big battery -->
+    <!-- <define name="G1_THRUST" value="{-0.3,   -0.3,   -0.45, -0.45, 0.0,   0.0,   0.0,  0.0}"/> -->
+    <!-- without big battery -->
+    <define name="G1_THRUST" value="{-0.37, -0.37,  -0.55, -0.55,   0.0,   0.0,   0.0,  0.0}"/>
+    <!--Counter torque effect of spinning up a rotor-->
+    <define name="G2" value="{0, 0, 0, 0, 0, 0, 0, 0}"/>
 
-    <!-- control effectiveness (forward) -->
-    <define name="FORWARD_G1_P" value="0.0020"/>
-    <define name="FORWARD_G1_Q" value="0.0077"/>
-    <define name="FORWARD_G1_R" value="0.004"/>
+    <!-- Forward gains -->
+    <define name="G1_ROLL_FWD"   value="{ 0.253, -0.253, 0.684, -0.684, 0.0,   0.0,   0.0,  0.0}"/>
+    <define name="G1_PITCH_FWD"  value="{ 0.3,    0.3,  -0.6,   -0.6,  -1.4,  -1.4,  -1.4, -1.4}"/>
+    <define name="G1_YAW_FWD"    value="{ 0.0,    0.0,   0.0,    0.0,   0.94, -0.94, -0.94, 0.94}"/>
+    <!-- with big battery -->
+    <!-- <define name="G1_THRUST_FWD" value="{-0.18   -0.18,  -0.27, -0.27,  0.0,   0.0,   0.0,  0.0}"/> -->
+    <!-- without big battery -->
+    <define name="G1_THRUST_FWD" value="{-0.22   -0.22,  -0.33, -0.33,  0.0,   0.0,   0.0,  0.0}"/>
 
     <!-- reference acceleration for attitude control -->
     <define name="REF_ERR_P" value="30.0"/>
@@ -295,7 +366,7 @@
     <define name="REF_RATE_R" value="6.0"/>
 
     <!--Maxium yaw rate, to avoid instability-->
-    <define name="MAX_R" value="50.0" unit="deg/s"/>
+    <define name="MAX_R" value="180.0" unit="deg/s"/>
 
     <!-- Maximum rate setpoint in rate control mode -->
     <define name="MAX_RATE" value="3.0" unit="rad/s"/>
@@ -304,12 +375,14 @@
     <define name="FILT_CUTOFF" value="1.5"/>
     <define name="FILT_CUTOFF_RDOT" value="0.5"/>
     <define name="ESTIMATION_FILT_CUTOFF" value="5.0"/>
+    <define name="FILT_CUTOFF_P" value="20."/>
+    <define name="FILT_CUTOFF_Q" value="20."/>
     <define name="FILT_CUTOFF_R" value="4.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.0354"/>
-    <define name="ACT_DYN_Q" value="0.0354"/>
-    <define name="ACT_DYN_R" value="0.0354"/>
+    <define name="ACT_DYN" value="{0.0354, 0.0354, 0.0354, 0.0354, 0.05, 0.05, 0.05, 0.05}"/>
+    <define name="ACT_RATE_LIMIT" value="{9600, 9600, 9600, 9600, 170, 170, 170, 170}"/>
+    <define name="ACT_IS_SERVO" value="{0, 0, 0, 0, 1, 1, 1, 1}"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
@@ -325,7 +398,7 @@
   </section>
 
   <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
-    <define name="MAX_BANK" value="30" unit="deg"/>
+    <define name="MAX_BANK" value="28" unit="deg"/>
     <define name="USE_SPEED_REF" value="TRUE"/>
     <define name="PGAIN" value="60"/>
     <define name="DGAIN" value="100"/>
@@ -334,10 +407,14 @@
   </section>
 
   <section name="SIMULATOR" prefix="NPS_">
-    <define name="ACTUATOR_NAMES" value="m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,m11,m12,ail1,ail2,ail3,ail4,flap1,flap2,flap3,flap4" type="string[]"/>
-    <define name="JSBSIM_MODEL" value="nederdrone" type="string"/>
+    <define name="ACTUATOR_NAMES" value="a0,a1,a2,a3,a4,a5,a6,a7,a8" type="string[]"/>
+    <define name="JSBSIM_MODEL" value="nederdrone4_tem" type="string"/>
+    <define name="NO_MOTOR_MIXING" value="TRUE"/>
+    <define name="COMMANDS_NB" value="9"/>
+    <define name="SENSORS_PARAMS" value="nps_sensors_params_default.h" type="string"/>
     <!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
     <define name="JS_AXIS_MODE" value="4"/>
+    <define name="DEBUG_SPEED_SP" value="false"/>
   </section>
 
   <section name="AUTOPILOT">
@@ -355,4 +432,4 @@
     <define name="BAT_NB_CELLS" value="6"/>
   </section>
 
-</airframe>
+</airframe>
\ No newline at end of file
diff --git a/conf/airframes/tudelft/nederdrone5.xml b/conf/airframes/tudelft/nederdrone5.xml
deleted file mode 100644
index 8e576d9875..0000000000
--- a/conf/airframes/tudelft/nederdrone5.xml
+++ /dev/null
@@ -1,371 +0,0 @@
-<!DOCTYPE airframe SYSTEM "../airframe.dtd">
-
-<!-- This is a Nedderdrone
-     * Airframe:    TUD00328
-     * Autopilot:   Pixhawk 4
-     * FBW:         Pixhawk 4
-     * Actuators:   12x T-Motor ESC + Motors and 8x Servos (all CAN)
-     * Datalink:    Doodlelabs 2.4GHz
-     * GPS:         UBlox F9P
-     * RC:          SBUS Crossfire
--->
-
-<airframe name="Neddrone5">
-  <description>Neddrone5</description>
-
-  <firmware name="rotorcraft">
-    <target name="ap" board="px4fmu_5.0_chibios">
-      <configure name="PERIODIC_FREQUENCY"  value="500"/>
-      <configure name="FLASH_MODE" value="SWD"/>
-
-      <module name="radio_control" type="sbus">
-        <configure name="SBUS_PORT" value="UART3"/>
-      </module>
-
-      <module name="airspeed" type="ms45xx_i2c">
-        <define name="USE_I2C4"/>
-        <define name="MS45XX_I2C_DEV" value="i2c4"/>
-      </module>
-
-      <module name="scheduling_indi_simple"/>
-
-      <!-- Forward FuelCell data back to the GCS -->
-      <!--module name="generic_uart_sensor"/-->
-
-      <!-- Logger -->
-      <module name="tlsf"/>
-      <module name="pprzlog"/>
-      <module name="logger" type="sd_chibios"/>
-      <module name="flight_recorder"/>
-
-      <define name="ADC_CURRENT_DISABLE" value="TRUE"/>
-
-      <define name="RADIO_TH_HOLD"     value="RADIO_AUX1"/> <!-- Throttle hold in command laws -->
-      <define name="RADIO_FMODE"       value="RADIO_AUX2"/> <!-- Throttle curve select -->
-      <define name="RADIO_FBW_MODE"    value="RADIO_AUX3"/> <!-- Switch between AP and FBW control -->
-      <define name="RADIO_KILL_SWITCH" value="RADIO_AUX1"/>
-
-      <!-- Use the external mag (not in NPS target as then it needs to listen to all) -->
-      <define name="INS_EKF2_MAG_ID" value="MAG_LIS3MDL_SENDER_ID"/>
-    </target>
-
-    <target name="nps" board="pc">
-      <module name="radio_control" type="datalink"/>
-      <module name="fdm" type="jsbsim"/>
-
-      <module name="scheduling_indi_simple"/>
-
-      <module name="logger_file">
-        <define name="LOGGER_FILE_PATH" value="/home/ewoud/Documents"/>
-      </module>
-
-      <!--Not dealing with these in the simulation-->
-      <define name="RADIO_TH_HOLD"     value="0"/> <!-- Throttle hold in command laws -->
-      <define name="RADIO_FMODE"       value="0"/> <!-- Throttle curve select -->
-      <define name="RADIO_FBW_MODE"    value="0"/> <!-- Switch between AP and FBW control -->
-      <define name="RADIO_KILL_SWITCH" value="0"/>
-    </target>
-
-    <module name="telemetry"     type="transparent">
-      <configure name="MODEM_BAUD" value="B115200"/>
-    </module>
-
-    <module name="approach_moving_target">
-      <define name="AMT_ERR_SLOWDOWN_GAIN" value="0.25"/>
-    </module>
-
-    <module name="actuators"     type="uavcan">
-      <configure name="UAVCAN_USE_CAN1" value="TRUE"/>
-      <configure name="UAVCAN_USE_CAN2" value="TRUE"/>
-    </module>
-    <module name="imu"           type="mpu6000"/>
-    <module name="gps"           type="ublox"/>
-    <module name="gps"           type="ubx_ucenter"/>
-    <module name="stabilization" type="indi_simple"/>
-    <module name="stabilization" type="rate_indi"/>
-    <module name="ins"           type="ekf2" />
-
-    <module name="air_data"/>
-
-    <!-- Internal MAG -->
-    <!--module name="mag_ist8310">
-      <define name="MODULE_IST8310_UPDATE_AHRS" value="TRUE"/>
-      <configure name="MAG_IST8310_I2C_DEV" value="I2C3"/>
-    </module-->
-    <!-- External MAG on GPS -->
-    <module name="mag_lis3mdl">
-      <define name="MODULE_LIS3MDL_UPDATE_AHRS" value="TRUE"/>
-      <configure name="MAG_LIS3MDL_I2C_DEV" value="I2C1"/>
-      <define name="LIS3MDL_CHAN_X_SIGN" value="-"/>
-      <define name="LIS3MDL_CHAN_Y_SIGN" value="-"/>
-    </module>
-    <!--module name="lidar" type="tfmini">
-      <configure name="TFMINI_PORT" value="UART4"/>
-      <configure name="USE_TFMINI_AGL" value="FALSE"/>
-    </module-->
-
-    <module name="nav" type="hybrid">
-      <define name="GUIDANCE_H_USE_REF" value="FALSE"/>
-    </module>
-
-    <module name="guidance" type="indi_hybrid">
-      <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
-      <define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
-      <define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
-      <define name="GUIDANCE_INDI_POS_GAINZ" value="0.2"/>
-      <define name="GUIDANCE_INDI_SPEED_GAINZ" value="1.0"/>
-      <define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
-      <define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
-      <define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
-      <define name="GUIDANCE_INDI_MAX_AIRSPEED" value="16.0"/>
-      <define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="5"/>
-      <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-943.0"/>
-      <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_45" value="-500.0"/>
-      <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_FWD" value="-1600.0"/>
-      <define name="GUIDANCE_INDI_FILTER_CUTOFF" value="0.5"/>
-      <define name="GUIDANCE_INDI_LINE_GAIN" value="0.2"/>
-      <define name="GUIDANCE_INDI_MIN_THROTTLE" value="2500"/>
-    </module>
-
-    <module name="motor_mixing"/>
-  </firmware>
-
-  <!-- CAN BUS 1 (Front Wing) -->
-  <servos driver="Uavcan1">
-    <servo name="MOTOR_1" no="0" min="-8191" neutral="1500" max="8191"/>
-    <servo name="MOTOR_2" no="1" min="-8191" neutral="1500" max="8191"/>
-    <servo name="MOTOR_3" no="2" min="-8191" neutral="1500" max="8191"/>
-    <servo name="MOTOR_4" no="3" min="-8191" neutral="1500" max="8191"/>
-    <servo name="MOTOR_5" no="4" min="-8191" neutral="1500" max="8191"/>
-    <servo name="MOTOR_6" no="5" min="-8191" neutral="1500" max="8191"/>
-    <servo name="AIL_1" no="6" min="6000" neutral="0" max="-6000"/>
-    <servo name="FLAP_1" no="7" min="6000" neutral="0" max="-6000"/>
-    <servo name="FLAP_2" no="8" min="-6000" neutral="0" max="6000"/>
-    <servo name="AIL_2" no="9" min="-6000" neutral="0" max="6000"/>
-  </servos>
-
-  <!-- CAN BUS 2 (Back Wing) -->
-  <servos driver="Uavcan2">
-    <servo name="MOTOR_7" no="0" min="-8191" neutral="1500" max="8191"/>
-    <servo name="MOTOR_8" no="1" min="-8191" neutral="1500" max="8191"/>
-    <servo name="MOTOR_9"  no="2" min="-8191" neutral="1500" max="8191"/>
-    <servo name="MOTOR_10" no="3" min="-8191" neutral="1500" max="8191"/>
-    <servo name="MOTOR_11" no="4" min="-8191" neutral="1500" max="8191"/>
-    <servo name="MOTOR_12" no="5" min="-8191" neutral="1500" max="8191"/>
-    <servo name="AIL_3" no="6" min="6000" neutral="0" max="-6000"/>
-    <servo name="FLAP_3" no="7" min="6000" neutral="0" max="-6000"/>
-    <servo name="FLAP_4" no="8" min="-6000" neutral="0" max="6000"/>
-    <servo name="AIL_4" no="9" min="-6000" neutral="0" max="6000"/>
-  </servos>
-
-  <commands>
-    <axis name="ROLL"   failsafe_value="0"/>
-    <axis name="PITCH"  failsafe_value="-300"/>
-    <axis name="YAW"    failsafe_value="0"/>
-    <axis name="THRUST" failsafe_value="0"/>
-  </commands>
-
-  <rc_commands>
-    <set command="THRUST" value="@THROTTLE" />
-    <set command="ROLL" value="@YAW" />
-    <set command="PITCH" value="@PITCH/2" />
-    <set command="YAW" value="-@ROLL/4" />
-  </rc_commands>
-
-  <section name="MIXING" prefix="MOTOR_MIXING_">
-    <!-- frontleft left (CCW), frontleft mid (CW), frontleft right (CCW), frontright left (CW), frontright mid (CCW), frontright right (CW) -->
-    <!-- backleft left (CW), backleft mid (CCW), backleft right (CW), backright left (CCW), backright mid (CW), backright right (CCW) -->
-    <define name="NB_MOTOR"    value="12"/>
-    <define name="SCALE"       value="256"/>
-    <define name="ROLL_COEF"   value="{256,  157,   56,  -56, -157, -256,  256,  157,   56,  -56, -157, -256}"/>
-    <define name="PITCH_COEF"  value="{256,  256,  256,  256,  256,  256, -256, -256, -256, -256, -256, -256}"/>
-    <define name="YAW_COEF"    value="{256, -256,  256, -256,  256, -256, -256,  256, -256,  256, -256,  256}"/>
-    <!--<define name="YAW_COEF"    value="{0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0}"/>-->
-    <define name="THRUST_COEF" value="{256,  256,  256,  256,  256,  256,  256,  256,  256,  256,  256,  256}"/>
-  </section>
-
-  <command_laws>
-    <call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
-    <let var="th_hold"     value="LessThan(RadioControlValues(RADIO_TH_HOLD), -4800)"/>
-
-    <set servo="MOTOR_1"   value="($th_hold? -9600 : motor_mixing.commands[0])"/>
-    <set servo="MOTOR_2"   value="($th_hold? -9600 : motor_mixing.commands[1])"/>
-    <set servo="MOTOR_3"   value="($th_hold? -9600 : motor_mixing.commands[2])"/>
-    <set servo="MOTOR_4"   value="($th_hold? -9600 : motor_mixing.commands[3])"/>
-    <set servo="MOTOR_5"   value="($th_hold? -9600 : motor_mixing.commands[4])"/>
-    <set servo="MOTOR_6"   value="($th_hold? -9600 : motor_mixing.commands[5])"/>
-    <set servo="MOTOR_7"   value="($th_hold? -9600 : motor_mixing.commands[6])"/>
-    <set servo="MOTOR_8"   value="($th_hold? -9600 : motor_mixing.commands[7])"/>
-    <set servo="MOTOR_9"   value="($th_hold? -9600 : motor_mixing.commands[8])"/>
-    <set servo="MOTOR_10"  value="($th_hold? -9600 : motor_mixing.commands[9])"/>
-    <set servo="MOTOR_11"  value="($th_hold? -9600 : motor_mixing.commands[10])"/>
-    <set servo="MOTOR_12"  value="($th_hold? -9600 : motor_mixing.commands[11])"/>
-
-    <!-- Removed ApplyDiff for differential control -->
-    <set servo="AIL_1"     value="-2*@PITCH + (  2*@YAW)"/>
-    <set servo="AIL_2"     value="-2*@PITCH + (- 2*@YAW)"/>
-    <set servo="AIL_3"     value=" 2*@PITCH + (  2*@YAW)"/>
-    <set servo="AIL_4"     value=" 2*@PITCH + (- 2*@YAW)"/>
-    <set servo="FLAP_1"    value="-2*@PITCH + (  2*@YAW)"/>
-    <set servo="FLAP_2"    value="-2*@PITCH + (- 2*@YAW)"/>
-    <set servo="FLAP_3"    value=" 2*@PITCH + (  2*@YAW)"/>
-    <set servo="FLAP_4"    value=" 2*@PITCH + (- 2*@YAW)"/>
-  </command_laws>
-
-  <section name="MISC">
-    <define name="VoltageOfAdc(adc)" value="((3.3f/4096.0f) * 18.9040120162 * adc)"/><!-- TODO: verify/calibrate -->
-    <define name="NO_RC_THRUST_LIMIT" value="TRUE"/>
-
-    <!-- Basic navigation settings -->
-    <define name="NAV_CLIMB_VSPEED" value="3.5"/>
-    <define name="NAV_DESCEND_VSPEED" value="-0.5"/>
-    <define name="ARRIVED_AT_WAYPOINT" value="50.0"/>
-
-    <!-- Avoid GPS loss behavior when having RC or datalink -->
-    <define name="NO_GPS_LOST_WITH_DATALINK_TIME" value="20"/>
-    <define name="NO_GPS_LOST_WITH_RC_VALID" value="TRUE"/>
-  </section>
-
-  <section name="FORWARD">
-    <!--The Nederdrone uses a slightly different axis system for the setpoint, to make both hovering and flying forward intuitive-->
-    <define name="USE_EARTH_BOUND_RC_SETPOINT" value="TRUE"/>
-    <!-- This is the pitch angle that the Nederdrone will have in forward flight, where 0 degrees is hover-->
-    <define name="TRANSITION_MAX_OFFSET" value="-80.0" unit="deg"/>
-    <!-- For RC coordinated turns, lower because the yawing was too slow -->
-    <define name="MAX_FWD_SPEED" value="20.0"/>
-    <!-- For hybrid guidance -->
-    <define name="MAX_AIRSPEED" value="20.0"/>
-    <!-- Enable airspeed measurements -->
-    <define name="USE_AIRSPEED" value="TRUE"/>
-  </section>
-
-  <section name="IMU" prefix="IMU_">
-    <!-- Rotate the IMU (for Pixhawk 4) -->
-    <define name="MPU_CHAN_X" value="1"/>
-    <define name="MPU_CHAN_Y" value="0"/>
-    <define name="MPU_CHAN_Z" value="2"/>
-    <define name="MPU_X_SIGN" value="1"/>
-    <define name="MPU_Y_SIGN" value="1"/>
-    <define name="MPU_Z_SIGN" value="-1"/>
-
-    <!-- Calibrated 09-03-2021 in MAVLab outside body -->
-    <define name="ACCEL_X_NEUTRAL" value="16"/>
-    <define name="ACCEL_Y_NEUTRAL" value="31"/>
-    <define name="ACCEL_Z_NEUTRAL" value="10"/>
-    <define name="ACCEL_X_SENS" value="4.793311542012962" integer="16"/>
-    <define name="ACCEL_Y_SENS" value="4.893892083504835" integer="16"/>
-    <define name="ACCEL_Z_SENS" value="4.791159603565934" integer="16"/>
-
-    <!-- NOT CALIBRATED (internal magnetometer) -->
-    <!--define name="MAG_X_NEUTRAL" value="-13"/>
-    <define name="MAG_Y_NEUTRAL" value="-15"/>
-    <define name="MAG_Z_NEUTRAL" value="12"/>
-    <define name="MAG_X_SENS" value="12.823826623678" integer="16"/>
-    <define name="MAG_Y_SENS" value="12.86305350064853" integer="16"/>
-    <define name="MAG_Z_SENS" value="12.512614596093622" integer="16"/-->
-
-    <!-- NOT CALIBRATED (external magnetometer) -->
-    <define name="MAG_X_NEUTRAL" value="2457"/>
-    <define name="MAG_Y_NEUTRAL" value="-1123"/>
-    <define name="MAG_Z_NEUTRAL" value="-1843"/>
-    <define name="MAG_X_SENS" value="0.590610116730755" integer="16"/>
-    <define name="MAG_Y_SENS" value="0.6411943049128617" integer="16"/>
-    <define name="MAG_Z_SENS" value="0.6279536066632421" integer="16"/>
-
-    <!-- Define axis in hover frame -->
-    <define name="BODY_TO_IMU_PHI"   value="0." unit="deg"/>
-    <define name="BODY_TO_IMU_THETA" value="90." unit="deg"/>
-    <define name="BODY_TO_IMU_PSI"   value="0." unit="deg"/>
-  </section>
-
-  <section name="STABILIZATION_ATTITUDE" prefix="STABILIZATION_ATTITUDE_">
-    <!-- setpoints -->
-    <define name="SP_MAX_PHI" value="60." unit="deg"/>
-    <define name="SP_MAX_THETA" value="80." 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"/>
-  </section>
-
-  <section name="STABILIZATION_ATTITUDE_INDI" prefix="STABILIZATION_INDI_">
-    <!-- control effectiveness (hover) -->
-    <define name="G1_P" value="0.0030"/>
-    <define name="G1_Q" value="0.0035"/>
-    <define name="G1_R" value="0.0004"/>
-    <define name="G2_R" value="0.00015"/>
-
-    <!-- control effectiveness (forward) -->
-    <define name="FORWARD_G1_P" value="0.0020"/>
-    <define name="FORWARD_G1_Q" value="0.0077"/>
-    <define name="FORWARD_G1_R" value="0.004"/>
-
-    <!-- reference acceleration for attitude control -->
-    <define name="REF_ERR_P" value="30.0"/>
-    <define name="REF_ERR_Q" value="30.0"/>
-    <define name="REF_ERR_R" value="20.0"/>
-    <define name="REF_RATE_P" value="6.0"/>
-    <define name="REF_RATE_Q" value="6.0"/>
-    <define name="REF_RATE_R" value="6.0"/>
-
-    <!--Maxium yaw rate, to avoid instability-->
-    <define name="MAX_R" value="50.0" unit="deg/s"/>
-
-    <!-- Maximum rate setpoint in rate control mode -->
-    <define name="MAX_RATE" value="3.0" unit="rad/s"/>
-
-    <!-- second order filter parameters -->
-    <define name="FILT_CUTOFF" value="1.5"/>
-    <define name="FILT_CUTOFF_RDOT" value="0.5"/>
-    <define name="ESTIMATION_FILT_CUTOFF" value="5.0"/>
-    <define name="FILT_CUTOFF_R" value="4.0"/>
-
-    <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.0354"/>
-    <define name="ACT_DYN_Q" value="0.0354"/>
-    <define name="ACT_DYN_R" value="0.0354"/>
-
-    <!-- Adaptive Learning Rate -->
-    <define name="USE_ADAPTIVE" value="FALSE"/>
-    <define name="ADAPTIVE_MU" value="0.0001"/>
-  </section>
-
-  <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
-    <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="ADAPT_THROTTLE_ENABLED" value="FALSE"/>
-  </section>
-
-  <section name="GUIDANCE_H" prefix="GUIDANCE_H_">
-    <define name="MAX_BANK" value="30" unit="deg"/>
-    <define name="USE_SPEED_REF" value="TRUE"/>
-    <define name="PGAIN" value="60"/>
-    <define name="DGAIN" value="100"/>
-    <define name="AGAIN" value="0"/>
-    <define name="IGAIN" value="20"/>
-  </section>
-
-  <section name="SIMULATOR" prefix="NPS_">
-    <define name="ACTUATOR_NAMES" value="m1,m2,m3,m4,m5,m6,m7,m8,m9,m10,m11,m12,ail1,ail2,ail3,ail4,flap1,flap2,flap3,flap4" type="string[]"/>
-    <define name="JSBSIM_MODEL" value="nederdrone" type="string"/>
-    <!-- mode switch on joystick channel 5 (axis numbering starting at zero) -->
-    <define name="JS_AXIS_MODE" value="4"/>
-  </section>
-
-  <section name="AUTOPILOT">
-    <define name="MODE_MANUAL" value="AP_MODE_ATTITUDE_DIRECT"/>
-    <define name="MODE_AUTO1"  value="AP_MODE_FORWARD"/>
-    <define name="MODE_AUTO2"  value="AP_MODE_NAV"/>
-    <define name="MODE_STARTUP"  value="AP_MODE_NAV"/>
-  </section>
-
-  <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="MAX_BAT_LEVEL" value="25.2" unit="V"/>
-    <define name="BAT_NB_CELLS" value="6"/>
-  </section>
-
-</airframe>
diff --git a/conf/airframes/tudelft/nederdrone6.xml b/conf/airframes/tudelft/nederdrone6.xml
index 7aec7060a6..202a948211 100644
--- a/conf/airframes/tudelft/nederdrone6.xml
+++ b/conf/airframes/tudelft/nederdrone6.xml
@@ -63,7 +63,6 @@
     </target>
 
     <target name="nps" board="pc">
-      <!-- <module name="eff_scheduling_nederdrone_dummy"/> -->
       <module name="radio_control" type="datalink"/>
       <module name="fdm" type="jsbsim"/>
 
@@ -78,10 +77,13 @@
       <define name="RADIO_KILL_SWITCH" value="0"/>
     </target>
 
-    <module name="eff_scheduling_nederdrone"/>
+    <module name="eff_scheduling_nederdrone">
+      <!-- Trims -->
+      <define name="INDI_SCHEDULING_TRIM_ELEVATOR" value="0"/>
+    </module>
 
     <module name="telemetry"     type="transparent">
-      <configure name="MODEM_BAUD" value="B115200"/>
+      <configure name="MODEM_BAUD" value="B460800"/>
     </module>
 
     <module name="approach_moving_target">
@@ -138,9 +140,11 @@
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>
       <define name="GUIDANCE_INDI_POS_GAINZ" value="0.3"/>
       <define name="GUIDANCE_INDI_SPEED_GAINZ" value="0.5"/>
+      <define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
       <define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
       <define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
       <define name="GUIDANCE_INDI_MAX_AIRSPEED" value="17.0"/>
+      <define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/> <!--heading can not be set by navigation-->
       <define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="5"/>
       <!--define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-943.0"/>
       <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_45" value="-500.0"/>
@@ -337,9 +341,9 @@
     <define name="FILT_CUTOFF" value="1.5"/>
     <define name="FILT_CUTOFF_RDOT" value="0.5"/>
     <define name="ESTIMATION_FILT_CUTOFF" value="5.0"/>
-    <define name="FILT_CUTOFF_R" value="4."/>
     <define name="FILT_CUTOFF_P" value="20."/>
     <define name="FILT_CUTOFF_Q" value="20."/>
+    <define name="FILT_CUTOFF_R" value="4."/>
     <!-- first order actuator dynamics -->
     <!--define name="ACT_DYN" value="{0.009, 0.009, 0.009, 0.009, 0.05, 0.05, 0.05, 0.05}"/-->
     <define name="ACT_DYN" value="{0.028, 0.028, 0.028, 0.028, 0.05, 0.05, 0.05, 0.05}"/>
diff --git a/conf/airframes/tudelft/nederdrone7.xml b/conf/airframes/tudelft/nederdrone7.xml
index be20bc2cdd..a033babb86 100644
--- a/conf/airframes/tudelft/nederdrone7.xml
+++ b/conf/airframes/tudelft/nederdrone7.xml
@@ -35,6 +35,7 @@
       <module name="airspeed" type="ms45xx_i2c">
         <define name="USE_I2C4"/>
         <define name="MS45XX_I2C_DEV" value="i2c4"/>
+        <define name="I2C4_CLOCK_SPEED" value="100000"/>
       </module>
       <!--module name="airspeed" type="uavcan"/-->
 
@@ -77,10 +78,13 @@
       <define name="RADIO_KILL_SWITCH" value="0"/>
     </target>
 
-    <module name="eff_scheduling_nederdrone"/>
+    <module name="eff_scheduling_nederdrone">
+      <!-- Trims -->
+      <define name="INDI_SCHEDULING_TRIM_ELEVATOR" value="0"/>
+    </module>
 
     <module name="telemetry"     type="transparent">
-      <configure name="MODEM_BAUD" value="B115200"/>
+      <configure name="MODEM_BAUD" value="B460800"/>
     </module>
 
     <module name="approach_moving_target">
@@ -137,9 +141,11 @@
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>
       <define name="GUIDANCE_INDI_POS_GAINZ" value="0.3"/>
       <define name="GUIDANCE_INDI_SPEED_GAINZ" value="0.5"/>
+      <define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
       <define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
       <define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
       <define name="GUIDANCE_INDI_MAX_AIRSPEED" value="17.0"/>
+      <define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/> <!--heading can not be set by navigation-->
       <define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="5"/>
       <!--define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-943.0"/>
       <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_45" value="-500.0"/>
diff --git a/conf/airframes/tudelft/nederdrone8.xml b/conf/airframes/tudelft/nederdrone8.xml
index 96b8a3768d..b28d3d3b94 100644
--- a/conf/airframes/tudelft/nederdrone8.xml
+++ b/conf/airframes/tudelft/nederdrone8.xml
@@ -23,7 +23,7 @@
       </module>
 
       <!-- Forward FuelCell data back to the GCS -->
-      <!--module name="generic_uart_sensor"/-->
+      <module name="generic_uart_sensor"/>
 
       <!-- Logger -->
       <module name="tlsf"/>
@@ -35,6 +35,7 @@
       <module name="airspeed" type="ms45xx_i2c">
         <define name="USE_I2C4"/>
         <define name="MS45XX_I2C_DEV" value="i2c4"/>
+        <define name="I2C4_CLOCK_SPEED" value="100000"/>
       </module>
       <!--module name="airspeed" type="uavcan"/-->
 
@@ -77,17 +78,23 @@
       <define name="RADIO_KILL_SWITCH" value="0"/>
     </target>
 
-    <module name="eff_scheduling_nederdrone"/>
+    <module name="eff_scheduling_nederdrone">
+      <!-- Trims -->
+      <define name="INDI_SCHEDULING_TRIM_ELEVATOR" value="1100"/>
+      <define name="INDI_SCHEDULING_PREF_FLAPS_FACTOR" value="1.0"/>
+    </module>
 
     <module name="telemetry"     type="transparent">
-      <configure name="MODEM_BAUD" value="B115200"/>
+      <configure name="MODEM_BAUD" value="B460800"/>
     </module>
 
     <module name="approach_moving_target">
       <define name="AMT_ERR_SLOWDOWN_GAIN" value="0.25"/>
     </module>
 
-    <module name="ins"           type="ekf2" />
+    <module name="ins"           type="ekf2">
+      <define name="USE_INS_NAV_INIT" value="FALSE"/>
+    </module>
 
     <module name="actuators"     type="uavcan">
       <configure name="UAVCAN_USE_CAN1" value="TRUE"/>
@@ -137,9 +144,11 @@
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="0.5"/>
       <define name="GUIDANCE_INDI_POS_GAINZ" value="0.3"/>
       <define name="GUIDANCE_INDI_SPEED_GAINZ" value="0.5"/>
+      <define name="GUIDANCE_INDI_PITCH_LIFT_EFF" value="0.12"/>
       <define name="GUIDANCE_INDI_PITCH_EFF_SCALING" value="1.0"/>
       <define name="GUIDANCE_H_REF_MAX_SPEED" value="18.0"/> <!--not used-->
       <define name="GUIDANCE_INDI_MAX_AIRSPEED" value="17.0"/>
+      <define name="GUIDANCE_HEADING_IS_FREE" value="FALSE"/> <!--heading can not be set by navigation-->
       <define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="5"/>
       <!--define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-943.0"/>
       <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN_45" value="-500.0"/>
@@ -344,13 +353,11 @@
     <define name="ACT_DYN" value="{0.028, 0.028, 0.028, 0.028, 0.05, 0.05, 0.05, 0.05}"/>
     <define name="ACT_RATE_LIMIT" value="{9600, 9600, 9600, 9600, 170, 170, 170, 170}"/>
     <define name="ACT_IS_SERVO" value="{0, 0, 0, 0, 1, 1, 1, 1}"/>
+    <define name="WLS_WU" value="{1.,1.,1.,1.,1.,1.,1.,1.}"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
     <define name="ADAPTIVE_MU" value="0.0001"/>
-
-    <!-- Trims -->
-    <define name="TRIM_ELEVATOR" value="1800"/>
   </section>
 
   <section name="GUIDANCE_V" prefix="GUIDANCE_V_">
diff --git a/conf/flight_plans/tudelft/nederdrone_terschelling.xml b/conf/flight_plans/tudelft/nederdrone_terschelling.xml
index 1ec702e05c..43ff695a66 100644
--- a/conf/flight_plans/tudelft/nederdrone_terschelling.xml
+++ b/conf/flight_plans/tudelft/nederdrone_terschelling.xml
@@ -66,12 +66,13 @@
       <set value="FALSE" var="force_forward"/>
       <call_once fun="NavResurrect()"/>
       <call_once fun="nav_set_heading_current()"/>
-      <attitude pitch="-60" roll="0" throttle="0" until="stage_time>1" vmode="throttle"/>
+      <attitude pitch="-45" roll="0" throttle="0" until="stage_time>1" vmode="throttle"/>
     </block>
     <block name="TakeoffLow" strip_button="Takeoff" strip_icon="takeoff.png">
       <exception cond="GetPosHeight() @GT 18.0" deroute="Takeoff"/>
       <call_once fun="nav_set_heading_current()"/>
-      <attitude pitch="-40." roll="0" throttle="0.8" vmode="throttle"/>
+      <call_once fun="autopilot_set_in_flight(true)"/>
+      <attitude pitch="-45." roll="0" throttle="0.8" vmode="throttle"/>
     </block>
     <block name="Takeoff" strip_button="Takeoff" strip_icon="takeoff.png">
       <exception cond="GetPosHeight() @GT 40.0" deroute="Standby"/>
@@ -89,7 +90,6 @@
     </block>
     <block name="go_p2">
       <set value="TRUE" var="force_forward"/>
-      <call_once fun="nav_set_heading_deg(90)"/>
       <go wp="p2"/>
       <deroute block="stay_p1"/>
     </block>
diff --git a/conf/flight_plans/tudelft/nederdrone_troia.xml b/conf/flight_plans/tudelft/nederdrone_troia.xml
index 1c6567bb7b..3e55c4fd27 100644
--- a/conf/flight_plans/tudelft/nederdrone_troia.xml
+++ b/conf/flight_plans/tudelft/nederdrone_troia.xml
@@ -9,6 +9,7 @@
     <waypoint lat="38.474472222222225" lon="-8.871222222222222" name="HOME"/>
     <waypoint lat="38.476515" lon="-8.868380" name="CLIMB"/>
     <waypoint lat="38.476331" lon="-8.869787" name="STDBY"/>
+    <waypoint lat="38.476331" lon="-8.865787" name="CIRCLE"/>
     <waypoint lat="38.476940" lon="-8.867501" name="p1"/>
     <waypoint lat="38.481886" lon="-8.856215" name="p2"/>
     <!-- <waypoint lat="38.478809" lon="-8.851799" name="p3"/> -->
@@ -136,12 +137,13 @@
       <set value="FALSE" var="force_forward"/>
       <call_once fun="NavResurrect()"/>
       <call_once fun="nav_set_heading_current()"/>
-      <attitude pitch="-50" roll="0" throttle="0.8" until="stage_time>0" vmode="throttle"/>
+      <attitude pitch="-45" roll="0" throttle="0.0" until="stage_time>1" vmode="throttle"/>
     </block>
     <block name="TakeoffLow">
       <exception cond="GetPosHeight() @GT 18.0" deroute="Takeoff"/>
+      <call_once fun="autopilot_set_in_flight(true)"/>
       <call_once fun="nav_set_heading_current()"/>
-      <attitude pitch="-45." roll="0" throttle="0.9" vmode="throttle"/>
+      <attitude pitch="-45." roll="0" throttle="0.8" vmode="throttle"/>
     </block>
     <block name="Takeoff">
       <exception cond="GetPosHeight() @GT 70.0" deroute="GO NORTH"/>
@@ -178,7 +180,6 @@
     </block>
     <block name="go_p2">
       <set value="TRUE" var="force_forward"/>
-      <call_once fun="nav_set_heading_deg(90)"/>
       <go wp="p2"/>
       <deroute block="stay_p1"/>
     </block>
@@ -192,9 +193,9 @@
       <exception cond="GpsFixValid()" deroute="GO NORTH"/>
       <attitude pitch="0" roll="0" climb="-1.0" vmode="climb"/>
     </block>
-    <block name="Oval">
+    <block name="Circle">
       <set value="TRUE" var="force_forward"/>
-      <oval p1="p1" p2="p2" radius="nav.radius"/>
+      <circle radius="nav.radius" wp="CIRCLE"/>
     </block>
     <block name="land here">
       <set value="FALSE" var="force_forward"/>
diff --git a/conf/flight_plans/tudelft/nederdrone_valkenburg.xml b/conf/flight_plans/tudelft/nederdrone_valkenburg.xml
index 7968968ceb..68b58691f8 100644
--- a/conf/flight_plans/tudelft/nederdrone_valkenburg.xml
+++ b/conf/flight_plans/tudelft/nederdrone_valkenburg.xml
@@ -63,14 +63,15 @@
       <call_once fun="NavKillThrottle()"/>
       <attitude pitch="0" roll="0" throttle="0" until="FALSE" vmode="throttle"/>
     </block>
-    <block name="Start Engine">
+    <block name="Start Engine" strip_button="Takeoff" strip_icon="takeoff.png">
       <set value="FALSE" var="force_forward"/>
       <call_once fun="NavResurrect()"/>
       <call_once fun="nav_set_heading_current()"/>
-      <attitude pitch="-65" roll="0" throttle="0" until="stage_time>1" vmode="throttle"/>
+      <attitude pitch="-45" roll="0" throttle="0.0" until="stage_time>1" vmode="throttle"/>
     </block>
-    <block name="TakeoffLow" strip_button="Takeoff" strip_icon="takeoff.png">
+    <block name="TakeoffLow">
       <exception cond="GetPosHeight() @GT 18.0" deroute="Takeoff"/>
+      <call_once fun="autopilot_set_in_flight(true)"/>
       <call_once fun="nav_set_heading_current()"/>
       <attitude pitch="-45." roll="0" throttle="0.8" vmode="throttle"/>
     </block>
@@ -106,7 +107,6 @@
     </block>
     <block name="go_p2">
       <set value="TRUE" var="force_forward"/>
-      <call_once fun="nav_set_heading_deg(90)"/>
       <go wp="p2"/>
       <deroute block="stay_p1"/>
     </block>