Implement leadlag filter for feedforward control and artificial motor lag

.vscode/launch.json

@@ -17,7 +17,7 @@
             "name": "MOCKUP docker",
             "type": "gdb",
             "request": "attach",
-            "executable": "${workspaceFolder}/obj/main/indiflight_MOCKUP.so",
+            //"executable": "${workspaceFolder}/obj/main/indiflight_MOCKUP.so",
             "target": "localhost:3333",
             "remote": true,
             "cwd": "${workspaceRoot}",

configs/profiles/CineRat.txt

@@ -49,7 +49,6 @@
 #define USE_LEARNER
 #define USE_NN_CONTROL
 
-
 ################################################
 # CRAFT SETTINGS (formerly known as "profile") #
 ################################################
@@ -204,6 +203,13 @@ indiprofile 0
 set indi_attitude_gains = 3000, 3000, 1000
 set indi_rate_gains     =  230,  230,  150
 
+# --- feedforward control config
+# H(z) = (b0 + b1*z^-1 + b2*z^-2) / (1 + a1*z^-1 + a2*z^-2)
+# Coefficient: unitless * 1000
+# Coefficient order:  b0, b1, b2, a1, a2  
+# set indi_ff_coefs = 0, 0, 0, 0, 0
+set indi_ff_coefs = 292, -292, 0, -1949, 950
+
 # attitude controller limits: (deg/s)
 set indi_attitude_max_tilt_rate = 800
 set indi_attitude_max_yaw_rate = 500
@@ -235,6 +241,11 @@ set indi_act_max = 100, 100, 100, 100                # %
 set indi_wls_act_preferred_state = 0,0,0,0
 set indi_wls_act_penalties = 1,1,1,1
 
+# lead lag filtering for motor Outputs
+set indi_use_motor_lead_lag = 0
+set indi_act_time_constant_true_ms = 25,25,25,25
+set indi_act_time_constant_des_ms = 50,50,50,50
+
 # fx, fy, fz is N/kg / 1 * 100.  Roll, pitch is Nm / (kgm^2) / 1 * 10.  Yaw is Nm / (kgm^2) / 1 * 10
 set indi_act_g1_fx = 0,0,0,0
 set indi_act_g1_fy = 0,0,0,0