diff --git a/conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml b/conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml
index 18e2c1f3d6..e90d9dc200 100644
--- a/conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_ark_hexa_1-8.xml
@@ -91,13 +91,14 @@ Aggie Air ARK
   </section>
 
   <command_laws>
+    <!-- Use SERVO_FRONT_IDX to know the actuators array number, and user SERVO_FRONT_DRIVER_NO to use the xx-value defined in servo no="xx" -->
     <call fun="motor_mixing_run(autopilot_get_motors_on(),FALSE,values)"/>
-    <set servo="FRONT"  value="motor_mixing.commands[SERVO_FRONT]"/>
-    <set servo="FRONT_RIGHT" value="motor_mixing.commands[SERVO_FRONT_RIGHT]"/>
-    <set servo="BACK_RIGHT"   value="motor_mixing.commands[SERVO_BACK_RIGHT]"/>
-    <set servo="BACK"    value="motor_mixing.commands[SERVO_BACK]"/>
-    <set servo="BACK_LEFT"  value="motor_mixing.commands[SERVO_BACK_LEFT]"/>
-    <set servo="FRONT_LEFT"   value="motor_mixing.commands[SERVO_FRONT_LEFT]"/>
+    <set servo="FRONT"  value="motor_mixing.commands[SERVO_FRONT_IDX]"/>
+    <set servo="FRONT_RIGHT" value="motor_mixing.commands[SERVO_FRONT_RIGHT_IDX]"/>
+    <set servo="BACK_RIGHT"   value="motor_mixing.commands[SERVO_BACK_RIGHT_IDX]"/>
+    <set servo="BACK"    value="motor_mixing.commands[SERVO_BACK_IDX]"/>
+    <set servo="BACK_LEFT"  value="motor_mixing.commands[SERVO_BACK_LEFT_IDX]"/>
+    <set servo="FRONT_LEFT"   value="motor_mixing.commands[SERVO_FRONT_LEFT_IDX]"/>
   </command_laws>
 
   <section name="IMU" prefix="IMU_">
diff --git a/conf/airframes/AGGIEAIR/aggieair_conf.xml b/conf/airframes/AGGIEAIR/aggieair_conf.xml
index bcdf15e116..867530d33a 100644
--- a/conf/airframes/AGGIEAIR/aggieair_conf.xml
+++ b/conf/airframes/AGGIEAIR/aggieair_conf.xml
@@ -7,7 +7,7 @@
    telemetry="telemetry/AGGIEAIR/aggieair_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic_geofence.xml"
    settings="settings/rotorcraft_basic.xml settings/nps.xml"
-   settings_modules="modules/battery_monitor.xml modules/gps.xml modules/stabilization_float_euler.xml modules/nav_basic_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml"
+   settings_modules="modules/battery_monitor.xml modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_rotorcraft.xml modules/stabilization_float_euler.xml"
    gui_color="#ffff954c0000"
   />
   <aircraft
diff --git a/conf/airframes/BR/bebop_indi.xml b/conf/airframes/BR/bebop_indi.xml
index 4b85e07a15..7de3ca2b1d 100644
--- a/conf/airframes/BR/bebop_indi.xml
+++ b/conf/airframes/BR/bebop_indi.xml
@@ -171,9 +171,9 @@
     <define name="FILT_CUTOFF" value="8.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.1"/>
-    <define name="ACT_DYN_Q" value="0.1"/>
-    <define name="ACT_DYN_R" value="0.1"/>
+    <define name="ACT_FREQ_P" value="53.9"/>
+    <define name="ACT_FREQ_Q" value="53.9"/>
+    <define name="ACT_FREQ_R" value="53.9"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/BR/bebop_indi_frog.xml b/conf/airframes/BR/bebop_indi_frog.xml
index 326fd9389d..1e4d4cc95a 100644
--- a/conf/airframes/BR/bebop_indi_frog.xml
+++ b/conf/airframes/BR/bebop_indi_frog.xml
@@ -174,9 +174,9 @@
     <define name="FILT_CUTOFF" value="8.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.1"/>
-    <define name="ACT_DYN_Q" value="0.1"/>
-    <define name="ACT_DYN_R" value="0.1"/>
+    <define name="ACT_FREQ_P" value="53.9"/>
+    <define name="ACT_FREQ_Q" value="53.9"/>
+    <define name="ACT_FREQ_R" value="53.9"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/BR/conf.xml b/conf/airframes/BR/conf.xml
index 77748963e3..f4a2bdc3ad 100644
--- a/conf/airframes/BR/conf.xml
+++ b/conf/airframes/BR/conf.xml
@@ -18,7 +18,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/modules/config_asctec_v2.xml"
-   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml modules/switch_servo.xml"
+   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_rotorcraft.xml modules/stabilization_int_quat.xml modules/switch_servo.xml"
    gui_color="white"
   />
   <aircraft
diff --git a/conf/airframes/BR/ladybird_kit_indi_bart.xml b/conf/airframes/BR/ladybird_kit_indi_bart.xml
index 381b411ec1..b348848806 100644
--- a/conf/airframes/BR/ladybird_kit_indi_bart.xml
+++ b/conf/airframes/BR/ladybird_kit_indi_bart.xml
@@ -142,9 +142,9 @@
     <define name="FILT_CUTOFF" value="8.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.03"/>
-    <define name="ACT_DYN_Q" value="0.03"/>
-    <define name="ACT_DYN_R" value="0.03"/>
+    <define name="ACT_FREQ_P" value="15.6"/>
+    <define name="ACT_FREQ_Q" value="15.6"/>
+    <define name="ACT_FREQ_R" value="15.6"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/BR/quadshot_asp21_FutabaPPMonUart1.xml b/conf/airframes/BR/quadshot_asp21_FutabaPPMonUart1.xml
index 6e230b70e4..315ecc5340 100644
--- a/conf/airframes/BR/quadshot_asp21_FutabaPPMonUart1.xml
+++ b/conf/airframes/BR/quadshot_asp21_FutabaPPMonUart1.xml
@@ -235,9 +235,9 @@
     <define name="FILT_CUTOFF" value="8.0"/>
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.03"/>
-    <define name="ACT_DYN_Q" value="0.03"/>
-    <define name="ACT_DYN_R" value="0.03"/>
+    <define name="ACT_FREQ_P" value="15.6"/>
+    <define name="ACT_FREQ_Q" value="15.6"/>
+    <define name="ACT_FREQ_R" value="15.6"/>
         <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="TRUE"/>
     <define name="ADAPTIVE_MU" value="0.0001"/>
diff --git a/conf/airframes/ENAC/conf_enac.xml b/conf/airframes/ENAC/conf_enac.xml
index cfa514ac9f..eec8ef6c03 100644
--- a/conf/airframes/ENAC/conf_enac.xml
+++ b/conf/airframes/ENAC/conf_enac.xml
@@ -29,7 +29,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/ENAC/crazyflie_multi_ranger_test.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/ahrs_madgwick.xml modules/air_data.xml modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/opticflow_pmw3901.xml modules/range_forcefield.xml modules/sonar_vl53l1x.xml modules/stabilization_int_quat.xml"
+   settings_modules="modules/ahrs_madgwick.xml modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_rotorcraft.xml modules/opticflow_pmw3901.xml modules/range_forcefield.xml modules/sonar_vl53l1x.xml modules/stabilization_int_quat.xml"
    gui_color="blue"
   />
   <aircraft
diff --git a/conf/airframes/ENAC/cyfoam.xml b/conf/airframes/ENAC/cyfoam.xml
index 31a5b4835a..f942842470 100644
--- a/conf/airframes/ENAC/cyfoam.xml
+++ b/conf/airframes/ENAC/cyfoam.xml
@@ -157,6 +157,11 @@
   </servos>
 
   <commands>
+    <axis name="ELEVON_LEFT"  failsafe_value="0"/>
+    <axis name="ELEVON_RIGHT" failsafe_value="0"/>
+    <axis name="MOTOR_RIGHT"  failsafe_value="0"/>
+    <axis name="MOTOR_LEFT"   failsafe_value="0"/>
+
     <axis name="PITCH"  failsafe_value="0"/>
     <axis name="ROLL"   failsafe_value="0"/>
     <axis name="YAW"    failsafe_value="0"/>
@@ -234,7 +239,7 @@
     <define name="FILT_CUTOFF" value="5.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.1, 0.1, 0.045, 0.045}"/>
+    <define name="ACT_FREQ" value="{52.7, 52.7, 23.0, 23.0}"/>
     <define name="ACT_RATE_LIMIT" value="{170, 170, 9600, 9600}"/>
     <define name="ACT_IS_SERVO" value="{1, 1, 0, 0}"/>
 
diff --git a/conf/airframes/ENAC/hoops_gen_ap.xml b/conf/airframes/ENAC/hoops_gen_ap.xml
index e0d3818e40..329a250178 100644
--- a/conf/airframes/ENAC/hoops_gen_ap.xml
+++ b/conf/airframes/ENAC/hoops_gen_ap.xml
@@ -218,9 +218,9 @@
     <define name="REF_RATE_R" value="28.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml b/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
index 798b517d88..3ff80aaf72 100644
--- a/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
+++ b/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
@@ -52,7 +52,7 @@
     </module>
     <module name="guidance" type="indi">
       <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
-      <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.03"/>
+      <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="30.5"/>
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
     </module>
 
@@ -189,7 +189,7 @@
     <!--Counter torque effect of spinning up a rotor-->
     <define name="G2" value="{150.0,   -150.0,  150.0,   -150.0 }"/>
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.03, 0.03, 0.03, 0.03}"/>
+    <define name="ACT_FREQ" value="{30.5, 30.5, 30.5, 30.5}"/>
     <define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
 
     <!--Priority for each axis (roll, pitch, yaw and thrust)-->
diff --git a/conf/airframes/ENAC/quadrotor/bebop2_fish.xml b/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
index e441cd67c8..4be32d3442 100644
--- a/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
+++ b/conf/airframes/ENAC/quadrotor/bebop2_fish.xml
@@ -168,9 +168,9 @@
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml b/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
index 145eaee83c..76bce3af3f 100644
--- a/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
+++ b/conf/airframes/ENAC/quadrotor/bebop2_fish_outdoor.xml
@@ -170,9 +170,9 @@
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml b/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml
index 6d44fc38e4..66e7e4eb7c 100644
--- a/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml
+++ b/conf/airframes/ENAC/quadrotor/crazyflie_2.1.xml
@@ -216,9 +216,9 @@
     <define name="FILT_CUTOFF_RDOT" value="8.0f"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.03149f"/>
-    <define name="ACT_DYN_Q" value="0.03149f"/>
-    <define name="ACT_DYN_R" value="0.03149f"/>
+    <define name="ACT_FREQ_P" value="16.4"/>
+    <define name="ACT_FREQ_Q" value="16.4"/>
+    <define name="ACT_FREQ_R" value="16.4"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/ENAC/quadrotor/kakute_f7.xml b/conf/airframes/ENAC/quadrotor/kakute_f7.xml
index 1507b3b549..4d481fa73e 100644
--- a/conf/airframes/ENAC/quadrotor/kakute_f7.xml
+++ b/conf/airframes/ENAC/quadrotor/kakute_f7.xml
@@ -212,9 +212,9 @@
     <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/ENAC/quadrotor/robobee.xml b/conf/airframes/ENAC/quadrotor/robobee.xml
index 7275463a08..971ddcd78b 100644
--- a/conf/airframes/ENAC/quadrotor/robobee.xml
+++ b/conf/airframes/ENAC/quadrotor/robobee.xml
@@ -51,7 +51,7 @@
     <module name="guidance" type="indi">
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="3.0"/>
       <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
-      <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.03"/>
+      <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="15.6"/>
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
     </module>
 
@@ -202,7 +202,7 @@
     <!--Counter torque effect of spinning up a rotor-->
     <define name="G2"        value="{100, -100, 100, -100.0 }"/>
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.02, 0.02, 0.02, 0.02}"/>
+    <define name="ACT_FREQ" value="{10.3, 10.3, 10.3, 10.3}"/>
     <define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
 
     <!--Priority for each axis (roll, pitch, yaw and thrust)-->
diff --git a/conf/airframes/ENAC/quadrotor/ulysse_indi.xml b/conf/airframes/ENAC/quadrotor/ulysse_indi.xml
index e1b380ad24..9e686f45d3 100644
--- a/conf/airframes/ENAC/quadrotor/ulysse_indi.xml
+++ b/conf/airframes/ENAC/quadrotor/ulysse_indi.xml
@@ -54,7 +54,7 @@
     </module>
     <module name="guidance" type="indi">
       <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
-      <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.03"/>
+      <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="30.5"/>
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
     </module>
 
@@ -183,7 +183,7 @@
     <!--Counter torque effect of spinning up a rotor-->
     <define name="G2" value="{150.0,   -150.0,  150.0,   -150.0 }"/>
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.03, 0.03, 0.03, 0.03}"/>
+    <define name="ACT_FREQ" value="{30.5, 30.5, 30.5, 30.5}"/>
     <define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
 
     <!--Priority for each axis (roll, pitch, yaw and thrust)-->
diff --git a/conf/airframes/KS/ks_bebop2_stereo.xml b/conf/airframes/KS/ks_bebop2_stereo.xml
index 1f2dbcae69..acf1e58771 100644
--- a/conf/airframes/KS/ks_bebop2_stereo.xml
+++ b/conf/airframes/KS/ks_bebop2_stereo.xml
@@ -160,9 +160,9 @@
     <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/KS/ks_mavtec1.xml b/conf/airframes/KS/ks_mavtec1.xml
index 325b89580d..6e7304e628 100644
--- a/conf/airframes/KS/ks_mavtec1.xml
+++ b/conf/airframes/KS/ks_mavtec1.xml
@@ -183,9 +183,9 @@
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.03"/>
-    <define name="ACT_DYN_Q" value="0.03"/>
-    <define name="ACT_DYN_R" value="0.03"/>
+    <define name="ACT_FREQ_P" value="15.6"/>
+    <define name="ACT_FREQ_Q" value="15.6"/>
+    <define name="ACT_FREQ_R" value="15.6"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/MM/bebop2_lum1_xbee.xml b/conf/airframes/MM/bebop2_lum1_xbee.xml
index 72c4ee5eb2..873e92b87a 100644
--- a/conf/airframes/MM/bebop2_lum1_xbee.xml
+++ b/conf/airframes/MM/bebop2_lum1_xbee.xml
@@ -185,9 +185,9 @@
     <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml b/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
index 50f629773e..e1be866eb0 100644
--- a/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
+++ b/conf/airframes/OPENUAS/openuas_3dr_iris_plus.xml
@@ -505,9 +505,9 @@ The most crucial part for the magnetometer calibration:
     <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.04"/>
-    <define name="ACT_DYN_Q" value="0.04"/>
-    <define name="ACT_DYN_R" value="0.04"/>
+    <define name="ACT_FREQ_P" value="20.9"/>
+    <define name="ACT_FREQ_Q" value="20.9"/>
+    <define name="ACT_FREQ_R" value="20.9"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/> <!-- make true if all works -->
diff --git a/conf/airframes/OPENUAS/openuas_bitcraze_crazyflie_2_1.xml b/conf/airframes/OPENUAS/openuas_bitcraze_crazyflie_2_1.xml
index 1a5365343c..1316da718d 100644
--- a/conf/airframes/OPENUAS/openuas_bitcraze_crazyflie_2_1.xml
+++ b/conf/airframes/OPENUAS/openuas_bitcraze_crazyflie_2_1.xml
@@ -273,9 +273,9 @@
     <define name="FILT_CUTOFF" value="8.0f"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.03149f"/>
-    <define name="ACT_DYN_Q" value="0.03149f"/>
-    <define name="ACT_DYN_R" value="0.03149f"/>
+    <define name="ACT_FREQ_P" value="16.4"/>/>
+    <define name="ACT_FREQ_Q" value="16.4"/>/>
+    <define name="ACT_FREQ_R" value="16.4"/>/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/OPENUAS/openuas_eachine_trashcan.xml b/conf/airframes/OPENUAS/openuas_eachine_trashcan.xml
index bee2ef0a2a..0195d2db98 100644
--- a/conf/airframes/OPENUAS/openuas_eachine_trashcan.xml
+++ b/conf/airframes/OPENUAS/openuas_eachine_trashcan.xml
@@ -392,9 +392,9 @@
         <define name="FILT_CUTOFF" value="5.0"/>
 
         <!-- first order actuator dynamics (indi_simple) -->
-        <define name="ACT_DYN_P" value="0.06"/>
-        <define name="ACT_DYN_Q" value="0.06"/>
-        <define name="ACT_DYN_R" value="0.06"/>
+        <define name="ACT_FREQ_P" value="31.7"/>
+        <define name="ACT_FREQ_Q" value="31.7"/>
+        <define name="ACT_FREQ_R" value="31.7"/>
 
         <!-- Adaptive Learning Rate -->
         <define name="USE_ADAPTIVE" value="TRUE"/>
diff --git a/conf/airframes/OPENUAS/openuas_emax_tinyhawk_ii.xml b/conf/airframes/OPENUAS/openuas_emax_tinyhawk_ii.xml
index 93797a3f9c..68090a7444 100644
--- a/conf/airframes/OPENUAS/openuas_emax_tinyhawk_ii.xml
+++ b/conf/airframes/OPENUAS/openuas_emax_tinyhawk_ii.xml
@@ -342,9 +342,9 @@
         <define name="FILT_CUTOFF" value="5.0"/>
 
         <!-- first order actuator dynamics (indi_simple) -->
-        <define name="ACT_DYN_P" value="0.06"/>
-        <define name="ACT_DYN_Q" value="0.06"/>
-        <define name="ACT_DYN_R" value="0.06"/>
+        <define name="ACT_FREQ_P" value="31.7"/>
+        <define name="ACT_FREQ_Q" value="31.7"/>
+        <define name="ACT_FREQ_R" value="31.7"/>
 
         <!-- Adaptive Learning Rate -->
         <define name="USE_ADAPTIVE" value="TRUE"/>
diff --git a/conf/airframes/OPENUAS/openuas_own_jetson_tx220.xml b/conf/airframes/OPENUAS/openuas_own_jetson_tx220.xml
index bc3d8c4603..c27828b409 100644
--- a/conf/airframes/OPENUAS/openuas_own_jetson_tx220.xml
+++ b/conf/airframes/OPENUAS/openuas_own_jetson_tx220.xml
@@ -388,9 +388,9 @@
         <define name="FILT_CUTOFF" value="5.0"/>
 
         <!-- first order actuator dynamics (indi_simple) -->
-        <define name="ACT_DYN_P" value="0.06"/>
-        <define name="ACT_DYN_Q" value="0.06"/>
-        <define name="ACT_DYN_R" value="0.06"/>
+        <define name="ACT_FREQ_P" value="31.7"/>
+        <define name="ACT_FREQ_Q" value="31.7"/>
+        <define name="ACT_FREQ_R" value="31.7"/>
 
         <!-- Adaptive Learning Rate -->
         <define name="USE_ADAPTIVE" value="TRUE"/>
diff --git a/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml b/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
index d0660661e3..d99aa8da8c 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_ardrone_2.xml
@@ -436,9 +436,9 @@ The most crucial part for the magnetometer calibration:
     <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/> <!-- make true if all works -->
diff --git a/conf/airframes/OPENUAS/openuas_parrot_bebop.xml b/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
index 1158bd0742..cddaace7c8 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_bebop.xml
@@ -67,7 +67,7 @@
       <module name="stabilization" type="indi">
         <define name="STABILIZATION_INDI_RPM_FEEDBACK" value="TRUE"/>
         <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
-        <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
+        <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="53.9"/>
         <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       </module>
     </target>
@@ -484,11 +484,11 @@ second order filter parameters -->
     <define name="FILT_CUTOFF_RDOT" value="4.0"/> -->
 
     <!-- first order actuator dynamics -->
- <!--   <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>-->
+ <!--   <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>-->
 
-    <define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
+    <define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="TRUE"/>
diff --git a/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml b/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
index e122d54636..42ed93fb1a 100644
--- a/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
+++ b/conf/airframes/OPENUAS/openuas_parrot_bebop_2.xml
@@ -492,9 +492,9 @@ The most crucial part for the magnetometer calibration:
     <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/> <!-- make true if all works -->
diff --git a/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml b/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
index f05d21d973..1910b961ad 100644
--- a/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
+++ b/conf/airframes/OPENUAS/openuas_transitionrobotics_quadshot.xml
@@ -586,9 +586,9 @@ The most crucial part for the magnetometer calibration:
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.03"/>
-    <define name="ACT_DYN_Q" value="0.03"/>
-    <define name="ACT_DYN_R" value="0.03"/>
+    <define name="ACT_FREQ_P" value="15.6"/>
+    <define name="ACT_FREQ_Q" value="15.6"/>
+    <define name="ACT_FREQ_R" value="15.6"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="TRUE"/>
diff --git a/conf/airframes/examples/bebop2_indi.xml b/conf/airframes/examples/bebop2_indi.xml
index ff7ce3484a..5d15bd651f 100644
--- a/conf/airframes/examples/bebop2_indi.xml
+++ b/conf/airframes/examples/bebop2_indi.xml
@@ -130,9 +130,9 @@
     <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml b/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
index 87a37cf459..f4021f73c7 100644
--- a/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
+++ b/conf/airframes/examples/bebop2_ukf_magnetometer_calibration.xml
@@ -162,9 +162,9 @@
     <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/examples/matek_h743_slim.xml b/conf/airframes/examples/matek_h743_slim.xml
index 37ce831341..d8d5f14f85 100644
--- a/conf/airframes/examples/matek_h743_slim.xml
+++ b/conf/airframes/examples/matek_h743_slim.xml
@@ -215,9 +215,9 @@
     <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/examples/matek_h7_rotorcraft_hitl.xml b/conf/airframes/examples/matek_h7_rotorcraft_hitl.xml
index f63110b2f2..1cfcc85fdf 100644
--- a/conf/airframes/examples/matek_h7_rotorcraft_hitl.xml
+++ b/conf/airframes/examples/matek_h7_rotorcraft_hitl.xml
@@ -214,9 +214,9 @@
     <define name="FILT_CUTOFF_R" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/examples/quadshot_asp21_FutabaPPMonUart1.xml b/conf/airframes/examples/quadshot_asp21_FutabaPPMonUart1.xml
index 7cdb2fb160..ddd1f13860 100644
--- a/conf/airframes/examples/quadshot_asp21_FutabaPPMonUart1.xml
+++ b/conf/airframes/examples/quadshot_asp21_FutabaPPMonUart1.xml
@@ -235,9 +235,9 @@
     <define name="FILT_CUTOFF" value="8.0"/>
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.03"/>
-    <define name="ACT_DYN_Q" value="0.03"/>
-    <define name="ACT_DYN_R" value="0.03"/>
+    <define name="ACT_FREQ_P" value="15.6"/>
+    <define name="ACT_FREQ_Q" value="15.6"/>
+    <define name="ACT_FREQ_R" value="15.6"/>
         <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="TRUE"/>
     <define name="ADAPTIVE_MU" value="0.0001"/>
diff --git a/conf/airframes/examples/quadshot_asp21_spektrum.xml b/conf/airframes/examples/quadshot_asp21_spektrum.xml
index 4f666694e9..e6aa93726e 100644
--- a/conf/airframes/examples/quadshot_asp21_spektrum.xml
+++ b/conf/airframes/examples/quadshot_asp21_spektrum.xml
@@ -233,9 +233,9 @@
     <define name="FILT_CUTOFF" value="8.0"/>
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.03"/>
-    <define name="ACT_DYN_Q" value="0.03"/>
-    <define name="ACT_DYN_R" value="0.03"/>
+    <define name="ACT_FREQ_P" value="15.6"/>
+    <define name="ACT_FREQ_Q" value="15.6"/>
+    <define name="ACT_FREQ_R" value="15.6"/>
         <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
     <define name="ADAPTIVE_MU" value="0.0001"/>
diff --git a/conf/airframes/examples/trashcan.xml b/conf/airframes/examples/trashcan.xml
index dbfe859c4a..69198e94d6 100644
--- a/conf/airframes/examples/trashcan.xml
+++ b/conf/airframes/examples/trashcan.xml
@@ -388,9 +388,9 @@
         <define name="FILT_CUTOFF" value="5.0"/>
 
         <!-- first order actuator dynamics (indi_simple) -->
-        <define name="ACT_DYN_P" value="0.06"/>
-        <define name="ACT_DYN_Q" value="0.06"/>
-        <define name="ACT_DYN_R" value="0.06"/>
+        <define name="ACT_FREQ_P" value="31.7"/>
+        <define name="ACT_FREQ_Q" value="31.7"/>
+        <define name="ACT_FREQ_R" value="31.7"/>
 
         <!-- Adaptive Learning Rate -->
         <define name="USE_ADAPTIVE" value="TRUE"/>
diff --git a/conf/airframes/tudelft/ardrone2_OF_hover.xml b/conf/airframes/tudelft/ardrone2_OF_hover.xml
index f82bcf887b..97ac6d9542 100644
--- a/conf/airframes/tudelft/ardrone2_OF_hover.xml
+++ b/conf/airframes/tudelft/ardrone2_OF_hover.xml
@@ -197,9 +197,9 @@
 	<define name="FILT_CUTOFF" value="3.2" />
 
 	<!-- first order actuator dynamics -->
-	<define name="ACT_DYN_P" value="0.06" />
-	<define name="ACT_DYN_Q" value="0.06" />
-	<define name="ACT_DYN_R" value="0.06" />
+	<define name="ACT_FREQ_P" value="31.7" />
+	<define name="ACT_FREQ_Q" value="31.7" />
+	<define name="ACT_FREQ_R" value="31.7" />
 
 	<!-- Adaptive Learning Rate -->
 	<define name="USE_ADAPTIVE" value="FALSE" />
diff --git a/conf/airframes/tudelft/ardrone2_indi.xml b/conf/airframes/tudelft/ardrone2_indi.xml
index afdebe0724..73c36cb385 100644
--- a/conf/airframes/tudelft/ardrone2_indi.xml
+++ b/conf/airframes/tudelft/ardrone2_indi.xml
@@ -152,9 +152,9 @@
     <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/ardrone2_optitrack.xml b/conf/airframes/tudelft/ardrone2_optitrack.xml
index 13536ac6ed..4d880613e5 100644
--- a/conf/airframes/tudelft/ardrone2_optitrack.xml
+++ b/conf/airframes/tudelft/ardrone2_optitrack.xml
@@ -126,9 +126,9 @@
     <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop2_indi.xml b/conf/airframes/tudelft/bebop2_indi.xml
index e52b1d2499..420a42a28d 100644
--- a/conf/airframes/tudelft/bebop2_indi.xml
+++ b/conf/airframes/tudelft/bebop2_indi.xml
@@ -115,7 +115,7 @@
     <define name="ESTIMATION_FILT_CUTOFF" value="3.2f"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.08f, 0.08f, 0.08f, 0.08f}"/>
+    <define name="ACT_FREQ" value="{42.7, 42.7, 42.7, 42.7}"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop2_indi_convergence.xml b/conf/airframes/tudelft/bebop2_indi_convergence.xml
index 8841cf854e..74a5a7169d 100644
--- a/conf/airframes/tudelft/bebop2_indi_convergence.xml
+++ b/conf/airframes/tudelft/bebop2_indi_convergence.xml
@@ -154,9 +154,9 @@
     <define name="FILT_CUTOFF_P" value="10.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop2_no_damping.xml b/conf/airframes/tudelft/bebop2_no_damping.xml
index cfa8484c5f..537e495b14 100644
--- a/conf/airframes/tudelft/bebop2_no_damping.xml
+++ b/conf/airframes/tudelft/bebop2_no_damping.xml
@@ -25,7 +25,7 @@
     <module name="guidance" type="indi">
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.5"/>
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="1.8"/>
-      <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.06"/>
+      <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="31.7"/>
     </module>
 
     <module name="ahrs" type="int_cmpl_quat">
@@ -150,9 +150,9 @@
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop2_no_damping_WLS.xml b/conf/airframes/tudelft/bebop2_no_damping_WLS.xml
index 8db2903ca8..90064e4f6d 100644
--- a/conf/airframes/tudelft/bebop2_no_damping_WLS.xml
+++ b/conf/airframes/tudelft/bebop2_no_damping_WLS.xml
@@ -24,7 +24,7 @@
     <module name="guidance" type="indi">
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.5"/>
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="1.8"/>
-      <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.06"/>
+      <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="31.7"/>
     </module>
 
     <module name="ahrs" type="int_cmpl_quat">
@@ -141,7 +141,7 @@
     <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
+    <define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop2_optitrack.xml b/conf/airframes/tudelft/bebop2_optitrack.xml
index 8d631bcfef..9268f125db 100644
--- a/conf/airframes/tudelft/bebop2_optitrack.xml
+++ b/conf/airframes/tudelft/bebop2_optitrack.xml
@@ -150,9 +150,9 @@
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
index 2dfae7ba2e..fafaaa6a24 100644
--- a/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
+++ b/conf/airframes/tudelft/bebop2_optitrack_visionfront.xml
@@ -191,9 +191,9 @@
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop2_undistort_front.xml b/conf/airframes/tudelft/bebop2_undistort_front.xml
index a49ba7200f..bb85324ebc 100644
--- a/conf/airframes/tudelft/bebop2_undistort_front.xml
+++ b/conf/airframes/tudelft/bebop2_undistort_front.xml
@@ -174,9 +174,9 @@
     <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop_OF_hover.xml b/conf/airframes/tudelft/bebop_OF_hover.xml
index 3e2adbdb3d..1771c7ff95 100644
--- a/conf/airframes/tudelft/bebop_OF_hover.xml
+++ b/conf/airframes/tudelft/bebop_OF_hover.xml
@@ -24,7 +24,7 @@
     <module name="ins" type="gps_passthrough"/>
     <!-- module name="guidance" type="indi"/-->
     <!-- <module name="guidance" type="indi"> <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" 
-        value="-500.0"/> <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/> 
+        value="-500.0"/> <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="53.9"/> 
         <define name="GUIDANCE_INDI_RC_DEBUG" value="TRUE"/> </module> -->
 
 
@@ -175,9 +175,9 @@
     <define name="FILT_CUTOFF" value="8.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.1"/>
-    <define name="ACT_DYN_Q" value="0.1"/>
-    <define name="ACT_DYN_R" value="0.1"/>
+    <define name="ACT_FREQ_P" value="53.9"/>
+    <define name="ACT_FREQ_Q" value="53.9"/>
+    <define name="ACT_FREQ_R" value="53.9"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop_autonomous_race_2018.xml b/conf/airframes/tudelft/bebop_autonomous_race_2018.xml
index 6b44a2e012..f0e28d5074 100644
--- a/conf/airframes/tudelft/bebop_autonomous_race_2018.xml
+++ b/conf/airframes/tudelft/bebop_autonomous_race_2018.xml
@@ -215,9 +215,9 @@
     <define name="FILT_ZETA_R" value="0.55"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.1"/>
-    <define name="ACT_DYN_Q" value="0.1"/>
-    <define name="ACT_DYN_R" value="0.1"/>
+    <define name="ACT_FREQ_P" value="53.9"/>
+    <define name="ACT_FREQ_Q" value="53.9"/>
+    <define name="ACT_FREQ_R" value="53.9"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop_course_orangeavoid.xml b/conf/airframes/tudelft/bebop_course_orangeavoid.xml
index dbf66b6598..b2045007f8 100644
--- a/conf/airframes/tudelft/bebop_course_orangeavoid.xml
+++ b/conf/airframes/tudelft/bebop_course_orangeavoid.xml
@@ -210,9 +210,9 @@
     <define name="ESTIMATION_FILT_CUTOFF" value="8.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.1"/>
-    <define name="ACT_DYN_Q" value="0.1"/>
-    <define name="ACT_DYN_R" value="0.1"/>
+    <define name="ACT_FREQ_P" value="53.9"/>
+    <define name="ACT_FREQ_Q" value="53.9"/>
+    <define name="ACT_FREQ_R" value="53.9"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml b/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
index 72fa6f7d08..0445e5685d 100644
--- a/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
+++ b/conf/airframes/tudelft/bebop_course_orangeavoid_guided.xml
@@ -224,9 +224,9 @@
     <define name="ESTIMATION_FILT_CUTOFF" value="8.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.1"/>
-    <define name="ACT_DYN_Q" value="0.1"/>
-    <define name="ACT_DYN_R" value="0.1"/>
+    <define name="ACT_FREQ_P" value="53.9"/>
+    <define name="ACT_FREQ_Q" value="53.9"/>
+    <define name="ACT_FREQ_R" value="53.9"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop_flip.xml b/conf/airframes/tudelft/bebop_flip.xml
index 9eea1c4cfc..601730fa77 100644
--- a/conf/airframes/tudelft/bebop_flip.xml
+++ b/conf/airframes/tudelft/bebop_flip.xml
@@ -172,9 +172,9 @@
     <define name="FILT_CUTOFF" value="8.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.1"/>
-    <define name="ACT_DYN_Q" value="0.1"/>
-    <define name="ACT_DYN_R" value="0.1"/>
+    <define name="ACT_FREQ_P" value="53.9"/>
+    <define name="ACT_FREQ_Q" value="53.9"/>
+    <define name="ACT_FREQ_R" value="53.9"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop_indi_actuators.xml b/conf/airframes/tudelft/bebop_indi_actuators.xml
index 7980c5e15d..8dbeaa5ea6 100644
--- a/conf/airframes/tudelft/bebop_indi_actuators.xml
+++ b/conf/airframes/tudelft/bebop_indi_actuators.xml
@@ -32,7 +32,7 @@
     <module name="ins" type="extended"/>
     <module name="guidance" type="indi">
       <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
-      <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
+      <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="53.9"/>
       <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
       <define name="WLS_N_U" value="4"/>
       <define name="WLS_N_V" value="4"/>
@@ -46,6 +46,11 @@
   </firmware>
 
   <commands>
+    <axis name="TOP_LEFT"     failsafe_value="0"/>
+    <axis name="TOP_RIGHT"    failsafe_value="0"/>
+    <axis name="BOTTOM_RIGHT" failsafe_value="0"/>
+    <axis name="BOTTOM_LEFT"  failsafe_value="0"/>
+
     <axis name="PITCH" failsafe_value="0"/>
     <axis name="ROLL" failsafe_value="0"/>
     <axis name="YAW" failsafe_value="0"/>
@@ -128,7 +133,7 @@
     <define name="FILT_CUTOFF" value="5.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
+    <define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="TRUE"/>
@@ -165,6 +170,7 @@
 
   <section name="SIMULATOR" prefix="NPS_">
     <define name="ACTUATOR_NAMES" value="nw_motor, ne_motor, se_motor, sw_motor" type="string[]"/>
+    <define name="COMMANDS_NB" value="4"/>
     <define name="JSBSIM_MODEL" value="bebop" type="string"/>
     <define name="NO_MOTOR_MIXING" value="TRUE"/>
   </section>
diff --git a/conf/airframes/tudelft/bebop_mavlink.xml b/conf/airframes/tudelft/bebop_mavlink.xml
index 930f6e9816..c141f648c4 100644
--- a/conf/airframes/tudelft/bebop_mavlink.xml
+++ b/conf/airframes/tudelft/bebop_mavlink.xml
@@ -182,9 +182,9 @@
     <define name="FILT_CUTOFF" value="8.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.1"/>
-    <define name="ACT_DYN_Q" value="0.1"/>
-    <define name="ACT_DYN_R" value="0.1"/>
+    <define name="ACT_FREQ_P" value="53.9"/>
+    <define name="ACT_FREQ_Q" value="53.9"/>
+    <define name="ACT_FREQ_R" value="53.9"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml b/conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml
index 97ac2fd8d8..8114035179 100644
--- a/conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml
+++ b/conf/airframes/tudelft/bebop_one_indi_generated_ekf2.xml
@@ -40,7 +40,7 @@
 
     <module name="guidance" type="indi">
        <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
-       <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
+       <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="53.9"/>
        <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
      </module>
 
@@ -127,7 +127,7 @@
     <define name="REF_RATE_R"             value="28.0"/>
     <define name="ESTIMATION_FILT_CUTOFF" value="4.0"/>
     <define name="FILT_CUTOFF"            value="5.0"/>
-    <define name="ACT_DYN"                value="{0.1, 0.1, 0.1, 0.1}"/>
+    <define name="ACT_FREQ"               value="{53.9, 53.9, 53.9, 53.9}"/>
     <define name="USE_ADAPTIVE"           value="TRUE"/>
     <define name="ADAPTIVE_MU"            value="0.0001"/>
     <define name="WLS_PRIORITIES"         value="{1000, 1000, 1, 100}"/>
diff --git a/conf/airframes/tudelft/bebop_opticflow.xml b/conf/airframes/tudelft/bebop_opticflow.xml
index 93b24cdafd..d30b62da09 100644
--- a/conf/airframes/tudelft/bebop_opticflow.xml
+++ b/conf/airframes/tudelft/bebop_opticflow.xml
@@ -155,9 +155,9 @@
     <define name="FILT_CUTOFF" value="8.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.1"/>
-    <define name="ACT_DYN_Q" value="0.1"/>
-    <define name="ACT_DYN_R" value="0.1"/>
+    <define name="ACT_FREQ_P" value="53.9"/>
+    <define name="ACT_FREQ_Q" value="53.9"/>
+    <define name="ACT_FREQ_R" value="53.9"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop_opticflow_indoor.xml b/conf/airframes/tudelft/bebop_opticflow_indoor.xml
index 7ee77857cd..552e1e4871 100644
--- a/conf/airframes/tudelft/bebop_opticflow_indoor.xml
+++ b/conf/airframes/tudelft/bebop_opticflow_indoor.xml
@@ -67,6 +67,11 @@
   </firmware>
 
   <commands>
+    <axis name="TOP_LEFT"     failsafe_value="0"/>
+    <axis name="TOP_RIGHT"    failsafe_value="0"/>
+    <axis name="BOTTOM_RIGHT" failsafe_value="0"/>
+    <axis name="BOTTOM_LEFT"  failsafe_value="0"/>
+
     <axis name="PITCH" failsafe_value="0"/>
     <axis name="ROLL" failsafe_value="0"/>
     <axis name="YAW" failsafe_value="0"/>
@@ -146,7 +151,7 @@
     <define name="FILT_CUTOFF" value="5.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
+    <define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml b/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml
index 95a15bbb01..9a88117bfc 100644
--- a/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml
+++ b/conf/airframes/tudelft/bebop_opticflow_indoor_2x_30hz.xml
@@ -179,7 +179,7 @@ pyramid level 2: 21 fps average, min=11fps
     <define name="FILT_CUTOFF" value="5.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
+    <define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop_optitrack.xml b/conf/airframes/tudelft/bebop_optitrack.xml
index 1a48348f72..2d26323920 100644
--- a/conf/airframes/tudelft/bebop_optitrack.xml
+++ b/conf/airframes/tudelft/bebop_optitrack.xml
@@ -34,6 +34,11 @@
   </firmware>
 
   <commands>
+    <axis name="TOP_LEFT"     failsafe_value="0"/>
+    <axis name="TOP_RIGHT"    failsafe_value="0"/>
+    <axis name="BOTTOM_RIGHT" failsafe_value="0"/>
+    <axis name="BOTTOM_LEFT"  failsafe_value="0"/>
+
     <axis name="PITCH" failsafe_value="0"/>
     <axis name="ROLL" failsafe_value="0"/>
     <axis name="YAW" failsafe_value="0"/>
@@ -120,7 +125,7 @@
     <define name="FILT_CUTOFF" value="5.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
+    <define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml b/conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
index 47bebc7224..d60ef14040 100644
--- a/conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
+++ b/conf/airframes/tudelft/bebop_ralphthesis2020_stereo.xml
@@ -220,9 +220,9 @@
     <define name="ESTIMATION_FILT_CUTOFF" value="8.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.1"/>
-    <define name="ACT_DYN_Q" value="0.1"/>
-    <define name="ACT_DYN_R" value="0.1"/>
+    <define name="ACT_FREQ_P" value="53.9"/>
+    <define name="ACT_FREQ_Q" value="53.9"/>
+    <define name="ACT_FREQ_R" value="53.9"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/cyfoam.xml b/conf/airframes/tudelft/cyfoam.xml
index ad8fa027ee..208a70f3c7 100644
--- a/conf/airframes/tudelft/cyfoam.xml
+++ b/conf/airframes/tudelft/cyfoam.xml
@@ -306,7 +306,7 @@
       <define name="GUIDANCE_INDI_MAX_AIRSPEED" value="16.0"/>
       <define name="GUIDANCE_INDI_HEADING_BANK_GAIN" value="15.0"/>
       <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
-      <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.045"/>
+      <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="23.6"/>
       <!--<define name="KNIFE_EDGE_TEST" value="TRUE"/>-->
       <!--Flap effectiveness on lift-->
       <define name="FE_LIFT_A_PITCH" value="0.00018"/>
diff --git a/conf/airframes/tudelft/disco_rotorcraft_indi.xml b/conf/airframes/tudelft/disco_rotorcraft_indi.xml
index c759e36508..723887eaa6 100644
--- a/conf/airframes/tudelft/disco_rotorcraft_indi.xml
+++ b/conf/airframes/tudelft/disco_rotorcraft_indi.xml
@@ -179,7 +179,7 @@
     <define name="FILT_CUTOFF" value="5.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.1, 0.1, 0.045}"/>
+    <define name="ACT_FREQ" value="{53.9, 53.9, 23.6}"/>
     <define name="ACT_RATE_LIMIT" value="{170, 170, 9600}"/>
     <define name="ACT_IS_SERVO" value="{1, 1, 0}"/>
 
diff --git a/conf/airframes/tudelft/fan_demo.xml b/conf/airframes/tudelft/fan_demo.xml
index 84e62c8f38..95f12fb160 100644
--- a/conf/airframes/tudelft/fan_demo.xml
+++ b/conf/airframes/tudelft/fan_demo.xml
@@ -101,7 +101,7 @@
 
   <section name="GUIDANCE_INDI">
     <define name="GUIDANCE_INDI_SPECIFIC_FORCE_GAIN" value="-500.0"/>
-    <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.1"/>
+    <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="53.9"/>
     <define name="GUIDANCE_INDI_RC_DEBUG" value="FALSE"/>
     <define name="GUIDANCE_INDI_POS_GAIN" value="0.7"/>
     <define name="GUIDANCE_INDI_SPEED_GAIN" value="1.5"/>
@@ -146,7 +146,7 @@
     <define name="FILT_CUTOFF" value="5.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}"/>
+    <define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="TRUE"/>
diff --git a/conf/airframes/tudelft/guido_ardrone2_optitrack.xml b/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
index 9a973a760a..5db8d9205b 100644
--- a/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
+++ b/conf/airframes/tudelft/guido_ardrone2_optitrack.xml
@@ -133,9 +133,9 @@ ARDrone2 with optical_flow landing.
     <define name="FILT_CUTOFF" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.06"/>
-    <define name="ACT_DYN_Q" value="0.06"/>
-    <define name="ACT_DYN_R" value="0.06"/>
+    <define name="ACT_FREQ_P" value="31.7"/>
+    <define name="ACT_FREQ_Q" value="31.7"/>
+    <define name="ACT_FREQ_R" value="31.7"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/hybrid_nav_test.xml b/conf/airframes/tudelft/hybrid_nav_test.xml
index 025080cd2a..f8afe30056 100644
--- a/conf/airframes/tudelft/hybrid_nav_test.xml
+++ b/conf/airframes/tudelft/hybrid_nav_test.xml
@@ -167,11 +167,10 @@
     <define name="FILT_CUTOFF_R" value="4.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.047"/>
-    <define name="ACT_DYN_Q" value="0.047"/>
-    <define name="ACT_DYN_R" value="0.047"/>
-    <define name="ACT_DYN_B" value="0.047"/>
-    <define name="ACT_DYN_P" value="0.047"/>
+    <define name="ACT_FREQ_P" value="24.1"/>
+    <define name="ACT_FREQ_Q" value="24.1"/>
+    <define name="ACT_FREQ_R" value="24.1"/>
+    <define name="ACT_FREQ_B" value="24.1"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/iris_indi.xml b/conf/airframes/tudelft/iris_indi.xml
index 464cbfa2f9..5d6b72f1c0 100644
--- a/conf/airframes/tudelft/iris_indi.xml
+++ b/conf/airframes/tudelft/iris_indi.xml
@@ -212,9 +212,9 @@
     <define name="FILT_CUTOFF" value="3.2"/>
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.04" />
-    <define name="ACT_DYN_Q" value="0.04" />
-    <define name="ACT_DYN_R" value="0.04" />
+    <define name="ACT_FREQ_P" value="20.9"/>
+    <define name="ACT_FREQ_Q" value="20.9"/>
+    <define name="ACT_FREQ_R" value="20.9"/>
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE" />
     <define name="ADAPTIVE_MU" value="0.0001" />
diff --git a/conf/airframes/tudelft/mavtec1.xml b/conf/airframes/tudelft/mavtec1.xml
index 5f66ab7dbd..530bd59ebb 100644
--- a/conf/airframes/tudelft/mavtec1.xml
+++ b/conf/airframes/tudelft/mavtec1.xml
@@ -187,9 +187,9 @@
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.03"/>
-    <define name="ACT_DYN_Q" value="0.03"/>
-    <define name="ACT_DYN_R" value="0.03"/>
+    <define name="ACT_FREQ_P" value="15.6"/>
+    <define name="ACT_FREQ_Q" value="15.6"/>
+    <define name="ACT_FREQ_R" value="15.6"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/mavtec5.xml b/conf/airframes/tudelft/mavtec5.xml
index 18d372344c..8084871176 100644
--- a/conf/airframes/tudelft/mavtec5.xml
+++ b/conf/airframes/tudelft/mavtec5.xml
@@ -177,9 +177,9 @@
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.15"/>
-    <define name="ACT_DYN_Q" value="0.15"/>
-    <define name="ACT_DYN_R" value="0.15"/>
+    <define name="ACT_FREQ_P" value="83.2"/>
+    <define name="ACT_FREQ_Q" value="83.2"/>
+    <define name="ACT_FREQ_R" value="83.2"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/nederdrone4.xml b/conf/airframes/tudelft/nederdrone4.xml
index 3081476353..85a9182180 100644
--- a/conf/airframes/tudelft/nederdrone4.xml
+++ b/conf/airframes/tudelft/nederdrone4.xml
@@ -67,7 +67,7 @@
       <module name="fdm" type="jsbsim"/>
 
       <module name="logger_file">
-        <define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
+        <define name="FILE_LOGGER_PATH" value="~/"/>
       </module>
 
       <!--Not dealing with these in the simulation-->
@@ -225,7 +225,6 @@
     <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="{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>
 
@@ -387,7 +386,7 @@
     <define name="FILT_CUTOFF_R" value="4.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.0354, 0.0354, 0.0354, 0.0354, 0.05, 0.05, 0.05, 0.05}"/>
+    <define name="ACT_FREQ" value="{18.0, 18.0, 18.0, 18.0, 25.6, 25.6, 25.6, 25.6}"/>
     <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.}"/>
diff --git a/conf/airframes/tudelft/nederdrone6.xml b/conf/airframes/tudelft/nederdrone6.xml
index c133a35d08..581dfc1b35 100644
--- a/conf/airframes/tudelft/nederdrone6.xml
+++ b/conf/airframes/tudelft/nederdrone6.xml
@@ -68,7 +68,7 @@
       <module name="fdm" type="jsbsim"/>
 
       <module name="logger_file">
-        <define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
+        <define name="FILE_LOGGER_PATH" value="~/"/>
       </module>
 
       <!--Not dealing with these in the simulation-->
@@ -190,6 +190,18 @@
   </servos>
 
   <commands>
+    <axis name="FRONT_LEFT"       failsafe_value="0"/>
+    <axis name="FRONT_RIGHT"      failsafe_value="0"/>
+    <axis name="REAR_LEFT"        failsafe_value="0"/>
+    <axis name="REAR_RIGHT"       failsafe_value="0"/>
+
+    <axis name="FRONT_LEFT_FLAP"  failsafe_value="0"/>
+    <axis name="FRONT_RIGHT_FLAP" failsafe_value="0"/>
+    <axis name="REAR_LEFT_FLAP"   failsafe_value="0"/>
+    <axis name="REAR_RIGHT_FLAP"  failsafe_value="0"/>
+
+    <axis name="TIP_THRUSTERS"    failsafe_value="0"/>
+
     <axis name="ROLL"   failsafe_value="0"/>
     <axis name="PITCH"  failsafe_value="-300"/>
     <axis name="YAW"    failsafe_value="0"/>
@@ -204,7 +216,6 @@
     <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="{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>
 
@@ -342,8 +353,8 @@
     <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}"/>
+    <!--define name="ACT_FREQ" value="{4.5, 4.5, 4.5, 4.5, 25.6, 25.6, 25.6, 25.6}"/-->
+    <define name="ACT_FREQ" value="{14.2, 14.2, 14.2, 14.2, 25.6, 25.6, 25.6, 25.6}"/>
     <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.}"/>
diff --git a/conf/airframes/tudelft/nederdrone7.xml b/conf/airframes/tudelft/nederdrone7.xml
index d7bc1fe5aa..806f07db9b 100644
--- a/conf/airframes/tudelft/nederdrone7.xml
+++ b/conf/airframes/tudelft/nederdrone7.xml
@@ -248,6 +248,18 @@
   </servos>
 
   <commands>
+    <axis name="FRONT_LEFT"       failsafe_value="0"/>
+    <axis name="FRONT_RIGHT"      failsafe_value="0"/>
+    <axis name="REAR_LEFT"        failsafe_value="0"/>
+    <axis name="REAR_RIGHT"       failsafe_value="0"/>
+
+    <axis name="FRONT_LEFT_FLAP"  failsafe_value="0"/>
+    <axis name="FRONT_RIGHT_FLAP" failsafe_value="0"/>
+    <axis name="REAR_LEFT_FLAP"   failsafe_value="0"/>
+    <axis name="REAR_RIGHT_FLAP"  failsafe_value="0"/>
+
+    <axis name="TIP_THRUSTERS"    failsafe_value="0"/>
+
     <axis name="ROLL"   failsafe_value="0"/>
     <axis name="PITCH"  failsafe_value="-300"/>
     <axis name="YAW"    failsafe_value="0"/>
@@ -262,7 +274,6 @@
     <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="{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>
 
@@ -398,10 +409,10 @@
     <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."/>
+    <define name="FILT_CUTOFF_R" value="4.0"/>
+
     <!-- 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}"/>
+    <define name="ACT_FREQ" value="{14.2, 14.2, 14.2, 14.2, 25.6, 25.6, 25.6, 25.6}"/>
     <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.}"/>
diff --git a/conf/airframes/tudelft/nederdrone8.xml b/conf/airframes/tudelft/nederdrone8.xml
index b4a1181f02..c64cdd2154 100644
--- a/conf/airframes/tudelft/nederdrone8.xml
+++ b/conf/airframes/tudelft/nederdrone8.xml
@@ -68,7 +68,7 @@
       <module name="fdm" type="jsbsim"/>
 
       <module name="logger_file">
-        <define name="FILE_LOGGER_PATH" value="/home/ewoud/Documents"/>
+        <define name="FILE_LOGGER_PATH" value="~/"/>
       </module>
 
       <!--Not dealing with these in the simulation-->
@@ -190,6 +190,18 @@
   </servos>
 
   <commands>
+    <axis name="FRONT_LEFT"       failsafe_value="0"/>
+    <axis name="FRONT_RIGHT"      failsafe_value="0"/>
+    <axis name="REAR_LEFT"        failsafe_value="0"/>
+    <axis name="REAR_RIGHT"       failsafe_value="0"/>
+
+    <axis name="FRONT_LEFT_FLAP"  failsafe_value="0"/>
+    <axis name="FRONT_RIGHT_FLAP" failsafe_value="0"/>
+    <axis name="REAR_LEFT_FLAP"   failsafe_value="0"/>
+    <axis name="REAR_RIGHT_FLAP"  failsafe_value="0"/>
+
+    <axis name="TIP_THRUSTERS"    failsafe_value="0"/>
+
     <axis name="ROLL"   failsafe_value="0"/>
     <axis name="PITCH"  failsafe_value="-300"/>
     <axis name="YAW"    failsafe_value="0"/>
@@ -204,7 +216,6 @@
     <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="{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>
 
@@ -343,7 +354,7 @@
     <define name="FILT_CUTOFF_R" value="4.0"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.028, 0.028, 0.028, 0.028, 0.05, 0.05, 0.05, 0.05}"/>
+    <define name="ACT_FREQ" value="{14.2, 14.2, 14.2, 14.2, 25.6, 25.6, 25.6, 25.6}"/>
     <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.}"/>
diff --git a/conf/airframes/tudelft/nederquad.xml b/conf/airframes/tudelft/nederquad.xml
index e58703e113..2029ae6612 100644
--- a/conf/airframes/tudelft/nederquad.xml
+++ b/conf/airframes/tudelft/nederquad.xml
@@ -218,9 +218,9 @@
     <define name="FILT_CUTOFF" value="1.5"/>
     <define name="FILT_CUTOFF_R" value="1.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_FREQ_P" value="18.0" />
+    <define name="ACT_FREQ_Q" value="18.0" />
+    <define name="ACT_FREQ_R" value="18.0" />
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE" />
     <define name="ADAPTIVE_MU" value="0.0001" />
diff --git a/conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml b/conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml
index c487f31cc1..5e49031f28 100644
--- a/conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml
+++ b/conf/airframes/tudelft/origami_lisamxs_wifi_indi_stereoboard.xml
@@ -248,9 +248,9 @@
     <define name="FILT_ZETA_R" value="0.55"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.03"/>
-    <define name="ACT_DYN_Q" value="0.03"/>
-    <define name="ACT_DYN_R" value="0.03"/>
+    <define name="ACT_FREQ_P" value="15.6"/>
+    <define name="ACT_FREQ_Q" value="15.6"/>
+    <define name="ACT_FREQ_R" value="15.6"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>
diff --git a/conf/airframes/tudelft/robird.xml b/conf/airframes/tudelft/robird.xml
index e780dde9ea..83839ec7f4 100644
--- a/conf/airframes/tudelft/robird.xml
+++ b/conf/airframes/tudelft/robird.xml
@@ -197,9 +197,9 @@ Flapping wing frame equiped with
     <define name="FILT_CUTOFF" value="3.2"/>
     <define name="FILT_CUTOFF_RDOT" value="3.2"/>
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.04" />
-    <define name="ACT_DYN_Q" value="0.04" />
-    <define name="ACT_DYN_R" value="0.04" />
+    <define name="ACT_FREQ_P" value="20.9"/>
+    <define name="ACT_FREQ_Q" value="20.9"/>
+    <define name="ACT_FREQ_R" value="20.9"/>
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE" />
     <define name="ADAPTIVE_MU" value="0.0001" />
diff --git a/conf/airframes/tudelft/rot_wing_25kg.xml b/conf/airframes/tudelft/rot_wing_25kg.xml
index ac0238c28c..11e62e4119 100644
--- a/conf/airframes/tudelft/rot_wing_25kg.xml
+++ b/conf/airframes/tudelft/rot_wing_25kg.xml
@@ -362,9 +362,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_DYN"              value="{0.024,  0.024,  0.024,  0.024,  0.1,  0.1, 0.1, 0.1, 0.047}"/>
-        <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="{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 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"/>
diff --git a/conf/airframes/tudelft/rot_wing_v3d.xml b/conf/airframes/tudelft/rot_wing_v3d.xml
index 4485dee5da..10c2756efa 100644
--- a/conf/airframes/tudelft/rot_wing_v3d.xml
+++ b/conf/airframes/tudelft/rot_wing_v3d.xml
@@ -222,11 +222,23 @@
     </servos>
 
     <commands>
-        <axis name="ROLL"       failsafe_value="0"/>
-        <axis name="PITCH"      failsafe_value="0"/>
-        <axis name="YAW"        failsafe_value="0"/>
-        <axis name="THRUST"     failsafe_value="0"/>
-        <axis name="THRUST_X"   failsafe_value="0"/>
+        <!-- commands from INDI -->
+        <axis name="FRONT_MOTOR" failsafe_value="0"/>
+        <axis name="RIGHT_MOTOR" failsafe_value="0"/>
+        <axis name="BACK_MOTOR"  failsafe_value="0"/>
+        <axis name="LEFT_MOTOR"  failsafe_value="0"/>
+        <axis name="RUDDER"      failsafe_value="0"/>
+        <axis name="ELEVATOR"    failsafe_value="0"/>
+        <axis name="AILERON"     failsafe_value="0"/>
+        <axis name="FLAP"        failsafe_value="0"/>
+        <axis name="THRUST_X"    failsafe_value="0"/>
+        <!-- commands modules -->
+        <axis name="SKEW"        failsafe_value="0"/>
+        <!-- default commands -->
+        <axis name="THRUST"      failsafe_value="0"/>
+        <axis name="ROLL"        failsafe_value="0"/>
+        <axis name="PITCH"       failsafe_value="0"/>
+        <axis name="YAW"         failsafe_value="0"/>
     </commands>
 
     <command_laws>
diff --git a/conf/airframes/tudelft/splash4.xml b/conf/airframes/tudelft/splash4.xml
index a8e063a101..a6027c8ae4 100644
--- a/conf/airframes/tudelft/splash4.xml
+++ b/conf/airframes/tudelft/splash4.xml
@@ -97,7 +97,7 @@
     <module name="guidance" type="indi">
       <define name="GUIDANCE_INDI_POS_GAIN" value="0.2"/>
       <define name="GUIDANCE_INDI_SPEED_GAIN" value="1.0"/>
-      <define name="GUIDANCE_INDI_THRUST_DYNAMICS" value="0.0354"/>
+      <define name="GUIDANCE_INDI_THRUST_DYNAMICS_FREQ" value="18.0"/>
     </module>
 
     <module name="air_data"/>
@@ -275,7 +275,7 @@
     <define name="FILTER_YAW_RATE" value="TRUE"/>
 
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN" value="{0.0354, 0.0354, 0.0354, 0.0354}"/>
+    <define name="ACT_FREQ" value="{18.0, 18.0, 18.0, 18.0}"/>
 
     <define name="WLS_PRIORITIES" value="{1000.f, 1000.f, 1.f, 100.f}"/>
 
diff --git a/conf/airframes/tudelft/splash_px4.xml b/conf/airframes/tudelft/splash_px4.xml
index 0598deff84..1ccc83d463 100644
--- a/conf/airframes/tudelft/splash_px4.xml
+++ b/conf/airframes/tudelft/splash_px4.xml
@@ -175,9 +175,9 @@
     <!-- second order filter parameters -->
     <define name="FILT_CUTOFF" value="3.2"/>
     <!-- first order actuator dynamics -->
-    <define name="ACT_DYN_P" value="0.04" />
-    <define name="ACT_DYN_Q" value="0.04" />
-    <define name="ACT_DYN_R" value="0.04" />
+    <define name="ACT_FREQ_P" value="20.9"/>
+    <define name="ACT_FREQ_Q" value="20.9"/>
+    <define name="ACT_FREQ_R" value="20.9"/>
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE" />
     <define name="ADAPTIVE_MU" value="0.0001" />
diff --git a/conf/conf_tests_coverity.xml b/conf/conf_tests_coverity.xml
index 7beae85f27..1be672db80 100644
--- a/conf/conf_tests_coverity.xml
+++ b/conf/conf_tests_coverity.xml
@@ -84,7 +84,7 @@
    telemetry="telemetry/default_fixedwing_imu.xml"
    flight_plan="flight_plans/nav_modules.xml"
    settings="settings/fixedwing_basic.xml settings/control/ctl_dash_loiter_trim.xml settings/nps.xml"
-   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/nav_smooth.xml modules/nav_survey_poly_osam.xml modules/stabilization_attitude_fw.xml"
+   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/digital_cam_common.xml modules/electrical.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_basic_fw.xml modules/imu_common.xml modules/nav_basic_fw.xml modules/nav_smooth.xml modules/nav_survey_poly_osam.xml modules/stabilization_attitude_fw.xml"
    gui_color="blue"
   />
   <aircraft
diff --git a/conf/modules/actuators_asctec_v2.xml b/conf/modules/actuators_asctec_v2.xml
index 9190539868..96670c7818 100644
--- a/conf/modules/actuators_asctec_v2.xml
+++ b/conf/modules/actuators_asctec_v2.xml
@@ -25,10 +25,10 @@
     <define name="USE_$(ACTUATORS_ASCTEC_V2_I2C_DEV_UPPER)"/>
     <file name="actuators_asctec_v2.c"/>
     <test>
-        <define name="SERVO_FRONT" value="0"/>
-        <define name="SERVO_BACK" value="1"/>
-        <define name="SERVO_LEFT" value="2"/>
-        <define name="SERVO_RIGHT" value="3"/>
+        <define name="SERVO_FRONT_IDX" value="0"/>
+        <define name="SERVO_BACK_IDX" value="1"/>
+        <define name="SERVO_LEFT_IDX" value="2"/>
+        <define name="SERVO_RIGHT_IDX" value="3"/>
         <define name="USE_I2C1"/>
         <define name="ACTUATORS_ASCTEC_V2_I2C_DEV" value="i2c1"/>
     </test>
diff --git a/conf/modules/stabilization_indi.xml b/conf/modules/stabilization_indi.xml
index 15208692d2..6706cf80cd 100644
--- a/conf/modules/stabilization_indi.xml
+++ b/conf/modules/stabilization_indi.xml
@@ -38,7 +38,7 @@
       <define name="STABILIZATION_INDI_FILT_CUTOFF_Q" value="20.0" description="First order cutoff freq for angular rate in Hz"/>
       <define name="STABILIZATION_INDI_FILT_CUTOFF_R" value="20.0" description="First order cutoff freq for angular rate in Hz"/>
       <define name="ESTIMATION_FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
-      <define name="ACT_DYN" value="{0.1, 0.1, 0.1, 0.1}" description="actuator dynamics"/>
+      <define name="ACT_FREQ" value="{53.9, 53.9, 53.9, 53.9}" description="actuator dynamics [rad/s]"/>
       <define name="ACT_IS_SERVO" value="{0,0,0,0}" description="1 for every actuator that is a servo"/>
       <define name="ACT_IS_THRUSTER_X" value="{0,0,0,0}" description="1 for every actuator that is a thruster in the body-x direction"/>
       <define name="ACT_RATE_LIMIT" value="{9600,9600,9600,9600}" description="rate limit in PPRZ units per timestep (depends on control frequency)"/>
diff --git a/conf/modules/stabilization_indi_simple.xml b/conf/modules/stabilization_indi_simple.xml
index c2755850f7..405870119b 100644
--- a/conf/modules/stabilization_indi_simple.xml
+++ b/conf/modules/stabilization_indi_simple.xml
@@ -40,9 +40,9 @@
       <define name="FILT_CUTOFF" value="8.0" description="second order filter cutoff frequency Hz"/>
       <define name="FILT_CUTOFF_RDOT" value="8.0" description="second order filter cutoff frequency rdot Hz"/>
       <define name="ESTIMATION_FILT_CUTOFF" value="8.0" description="second order cutoff parameter"/>
-      <define name="ACT_DYN_P" value="0.1" description="first order actuator dynamics on roll rate"/>
-      <define name="ACT_DYN_Q" value="0.1" description="first order actuator dynamics on pitch rate"/>
-      <define name="ACT_DYN_R" value="0.1" description="first order actuator dynamics on yaw rate"/>
+      <define name="ACT_FREQ_P" value="53.9" description="first order actuator dynamics on roll rate [rad/s]"/>
+      <define name="ACT_FREQ_Q" value="53.9" description="first order actuator dynamics on pitch rate [rad/s]"/>
+      <define name="ACT_FREQ_R" value="53.9" description="first order actuator dynamics on yaw rate [rad/s]"/>
       <define name="USE_ADAPTIVE" value="FALSE|TRUE" description="enable adaptive gains"/>
       <define name="ADAPTIVE_MU" value="0.0001" description="adaptation parameter"/>
       <define name="FULL_AUTHORITY" value="FALSE" description="Enable full control authority"/>
diff --git a/conf/userconf/tudelft/conf.xml b/conf/userconf/tudelft/conf.xml
index 5eacc1e6a9..526b5b7e4f 100644
--- a/conf/userconf/tudelft/conf.xml
+++ b/conf/userconf/tudelft/conf.xml
@@ -7,7 +7,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_optitrack.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/ahrs_int_cmpl_quat.xml [modules/cv_opticflow.xml] modules/gps.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/optical_flow_hover.xml modules/stabilization_indi_simple.xml"
+   settings_modules="modules/ahrs_int_cmpl_quat.xml [modules/cv_opticflow.xml] modules/electrical.xml modules/gps.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_rotorcraft.xml modules/optical_flow_hover.xml modules/stabilization_indi_simple.xml"
    gui_color="red"
   />
   <aircraft
@@ -18,7 +18,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/gps_ubx_ucenter.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_basic_rotorcraft.xml modules/stabilization_int_quat.xml modules/video_rtp_stream.xml"
+   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/gps_ubx_ucenter.xml modules/guidance_pid_rotorcraft.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/ins_extended.xml modules/nav_rotorcraft.xml modules/stabilization_int_quat.xml modules/video_rtp_stream.xml"
    gui_color="#ffffd633d633"
   />
   <aircraft
@@ -260,7 +260,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/delft_basic.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/geo_mag.xml modules/gps.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_basic_rotorcraft.xml modules/stabilization_indi_simple.xml"
+   settings_modules="modules/ahrs_int_cmpl_quat.xml modules/air_data.xml modules/electrical.xml modules/geo_mag.xml modules/gps.xml modules/gps_ublox.xml modules/guidance_indi.xml modules/guidance_rotorcraft.xml modules/imu_common.xml modules/nav_rotorcraft.xml modules/stabilization_indi_simple.xml"
    gui_color="#ffff00000000"
   />
   <aircraft
@@ -282,7 +282,7 @@
    telemetry="telemetry/highspeed_rotorcraft.xml"
    flight_plan="flight_plans/tudelft/nederdrone_cyberzoo.xml"
    settings="settings/rotorcraft_basic.xml"
-   settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.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/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_chirp.xml modules/sys_id_doublet.xml modules/target_pos.xml"
+   settings_modules="modules/AOA_pwm.xml modules/air_data.xml modules/airspeed_ms45xx_i2c.xml modules/approach_moving_target.xml modules/eff_scheduling_nederdrone.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/ins_ekf2.xml modules/logger_sd_chibios.xml modules/nav_hybrid.xml modules/nav_rotorcraft.xml modules/preflight_checks.xml modules/stabilization_indi.xml modules/sys_id_auto_doublets.xml modules/sys_id_chirp.xml modules/sys_id_doublet.xml modules/target_pos.xml"
    gui_color="blue"
   />
   <aircraft
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
index 259431e600..c742da4995 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c
@@ -88,13 +88,11 @@ static void guidance_indi_filter_thrust(void);
 #endif
 
 #ifndef GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
-#ifndef GUIDANCE_INDI_THRUST_DYNAMICS
 #ifndef STABILIZATION_INDI_ACT_FREQ_P
-#error "You need to define GUIDANCE_INDI_THRUST_DYN_FREQ to be able to use indi vertical control"
+#error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ to be able to use indi vertical control"
 #else // assume that the same actuators are used for thrust as for roll (e.g. quadrotor)
 #define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_FREQ_P
 #endif
-#endif // GUIDANCE_INDI_THRUST_DYN_FREQ
 #endif //GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
 
 
@@ -176,11 +174,13 @@ void guidance_indi_enter(void)
   thrust_in = stabilization_cmd[COMMAND_THRUST];
   thrust_act = thrust_in;
 
+#ifdef GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
 #ifdef GUIDANCE_INDI_THRUST_DYNAMICS
   thrust_dyn = GUIDANCE_INDI_THRUST_DYNAMICS;
 #else
   thrust_dyn = 1-exp(-GUIDANCE_INDI_THRUST_DYNAMICS_FREQ/PERIODIC_FREQUENCY);
-#endif
+#endif //GUIDANCE_INDI_THRUST_DYNAMICS
+#endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
 
   float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
   float sample_time = 1.0 / PERIODIC_FREQUENCY;
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
index b31903ea22..ee4fa76bd5 100644
--- a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
+++ b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -127,8 +127,7 @@ static void guidance_indi_filter_thrust(void);
 #warning GUIDANCE_INDI_THRUST_DYNAMICS is deprecated, use GUIDANCE_INDI_THRUST_DYNAMICS_FREQ instead.
 #warning "The thrust dynamics are now specified in continuous time with the corner frequency of the first order model!"
 #warning "define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ in rad/s"
-#warning "Use -log(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old value."
-#define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ (-ln(1-GUIDANCE_INDI_THRUST_DYNAMICS)*PERIODIC_FREQUENCY)
+#warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old value."
 #endif
 
 #ifndef GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
diff --git a/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h
index b1152d5184..406abd0cc4 100644
--- a/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h
+++ b/sw/airborne/firmwares/rover/guidance/rover_guidance_steering.h
@@ -34,11 +34,11 @@
 #include "generated/airframe.h"
 
 // Check critical global definitiones
-#ifndef SERVO_MOTOR_THROTTLE
+#ifndef SERVO_MOTOR_THROTTLE_IDX
 #error "Steering rover firmware requires the servo MOTOR_THROTTLE"
 #endif
 
-#ifndef SERVO_MOTOR_STEERING
+#ifndef SERVO_MOTOR_STEERING_IDX
 #error "Steering rover firmware requires the servo MOTOR_STEERING"
 #endif
 
diff --git a/sw/airborne/modules/actuators/actuators_asctec_v2.c b/sw/airborne/modules/actuators/actuators_asctec_v2.c
index 01ca97e5ff..4419702116 100644
--- a/sw/airborne/modules/actuators/actuators_asctec_v2.c
+++ b/sw/airborne/modules/actuators/actuators_asctec_v2.c
@@ -109,10 +109,10 @@ void actuators_asctec_v2_set(void)
       actuators_asctec_v2.i2c_trans.len_w = 4;
       break;
     case NONE:
-      actuators_asctec_v2.i2c_trans.buf[0] = actuators_asctec_v2.cmds[SERVO_FRONT];
-      actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cmds[SERVO_BACK];
-      actuators_asctec_v2.i2c_trans.buf[2] = actuators_asctec_v2.cmds[SERVO_LEFT];
-      actuators_asctec_v2.i2c_trans.buf[3] = actuators_asctec_v2.cmds[SERVO_RIGHT];
+      actuators_asctec_v2.i2c_trans.buf[0] = actuators_asctec_v2.cmds[SERVO_FRONT_IDX];
+      actuators_asctec_v2.i2c_trans.buf[1] = actuators_asctec_v2.cmds[SERVO_BACK_IDX];
+      actuators_asctec_v2.i2c_trans.buf[2] = actuators_asctec_v2.cmds[SERVO_LEFT_IDX];
+      actuators_asctec_v2.i2c_trans.buf[3] = actuators_asctec_v2.cmds[SERVO_RIGHT_IDX];
       actuators_asctec_v2.i2c_trans.buf[4] = 0xAA + actuators_asctec_v2.i2c_trans.buf[0] +
                                              actuators_asctec_v2.i2c_trans.buf[1] +
                                              actuators_asctec_v2.i2c_trans.buf[2] +
diff --git a/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c b/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c
index b9d6b0d9d4..171716261c 100644
--- a/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c
+++ b/sw/airborne/modules/ctrl/eff_scheduling_nederdrone.c
@@ -184,6 +184,7 @@ void schdule_control_effectiveness(void) {
 #error "ctfl_eff_scheduling_nederdrone is very specific and only works for one Nederdrone configuration!"
 #endif
     if (i>3) {
+      // Increase servo effectiveness depending on the thrust of the propeller of that wing: 0,1,2,3 = motors, 4,5,6,7 = flaps
       float wing_thrust = actuators_pprz[i-4];
       Bound(wing_thrust,3000.0,9600.0);
       wing_thrust_scaling = wing_thrust/9600.0/0.8;
diff --git a/sw/simulator/nps/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h
index 7c04b3cd5e..c227e51845 100644
--- a/sw/simulator/nps/nps_autopilot.h
+++ b/sw/simulator/nps/nps_autopilot.h
@@ -39,7 +39,7 @@ extern "C" {
 #if defined MOTOR_MIXING_NB_MOTOR
 #define NPS_COMMANDS_NB MOTOR_MIXING_NB_MOTOR
 #else
-#define NPS_COMMANDS_NB COMMANDS_NB
+#define NPS_COMMANDS_NB ACTUATORS_NB  // uses actuators_pprz[ACTUATORS_NB]
 #endif /* #if defined MOTOR_MIXING_NB_MOTOR */
 #endif /* #ifndef NPS_COMMANDS_NB */