INDI actuator dynamics in continuous time (#3190)

* change INDI to use continuous time actuator frequency

* change to continuous time act dynamics definitions

- guidance_indi
- guidance_indi_hybrid
- stabilization_indi_simple

* keep the old values as deprecated default

conf/airframes/tudelft/bebop_indi.xml

@@ -156,9 +156,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="52.0"/>
+    <define name="ACT_FREQ_Q" value="52.0"/>
+    <define name="ACT_FREQ_R" value="52.0"/>
 
     <!-- Adaptive Learning Rate -->
     <define name="USE_ADAPTIVE" value="FALSE"/>

conf/airframes/tudelft/cyfoam.xml

@@ -111,7 +111,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="{53., 53., 23., 23.}"/>
     <define name="ACT_RATE_LIMIT" value="{170, 170, 9600, 9600}"/>
     <define name="ACT_IS_SERVO" value="{1, 1, 0, 0}"/>
 

conf/userconf/tudelft/conf.xml

@@ -95,7 +95,7 @@
    telemetry="telemetry/default_rotorcraft.xml"
    flight_plan="flight_plans/rotorcraft_basic.xml"
    settings="settings/rotorcraft_basic.xml settings/control/rotorcraft_speed.xml"
-   settings_modules="modules/ahrs_float_mlkf.xml modules/air_data.xml modules/bebop_ae_awb.xml modules/bebop_cam.xml modules/geo_mag.xml modules/gps.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_float_mlkf.xml modules/air_data.xml modules/bebop_ae_awb.xml modules/bebop_cam.xml modules/electrical.xml modules/geo_mag.xml modules/gps.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="#ffffbc3bce5b"
   />
   <aircraft

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi.c

@@ -80,13 +80,21 @@ struct FloatVect3 sp_accel = {0.0f, 0.0f, 0.0f};
 float guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
 static void guidance_indi_filter_thrust(void);
 
-#ifndef GUIDANCE_INDI_THRUST_DYNAMICS
-#ifndef STABILIZATION_INDI_ACT_DYN_P
-#error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS to be able to use indi vertical control"
+#ifdef GUIDANCE_INDI_THRUST_DYNAMICS
+#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."
+#endif
+
+#ifndef GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
+#ifndef STABILIZATION_INDI_ACT_FREQ_P
+#error "You need to define GUIDANCE_INDI_THRUST_DYN_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 STABILIZATION_INDI_ACT_DYN_P
+#define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_FREQ_P
 #endif
-#endif //GUIDANCE_INDI_THRUST_DYNAMICS
+#endif //GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
+
 
 #endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
 
@@ -98,7 +106,8 @@ static void guidance_indi_filter_thrust(void);
 #endif
 #endif
 
-float thrust_act = 0;
+float thrust_dyn = 0.f;
+float thrust_act = 0.f;
 Butterworth2LowPass filt_accel_ned[3];
 Butterworth2LowPass roll_filt;
 Butterworth2LowPass pitch_filt;
@@ -165,6 +174,12 @@ void guidance_indi_enter(void)
   thrust_in = stabilization_cmd[COMMAND_THRUST];
   thrust_act = thrust_in;
 
+#ifdef GUIDANCE_INDI_THRUST_DYNAMICS
+  thrust_dyn = GUIDANCE_INDI_THRUST_DYNAMICS;
+#else
+  thrust_dyn = 1-exp(-GUIDANCE_INDI_THRUST_DYNAMICS_FREQ/PERIODIC_FREQUENCY);
+#endif
+
   float tau = 1.0 / (2.0 * M_PI * filter_cutoff);
   float sample_time = 1.0 / PERIODIC_FREQUENCY;
   for (int8_t i = 0; i < 3; i++) {

sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c

@@ -123,13 +123,20 @@ struct FloatVect3 sp_accel = {0.0,0.0,0.0};
 float guidance_indi_specific_force_gain = GUIDANCE_INDI_SPECIFIC_FORCE_GAIN;
 static void guidance_indi_filter_thrust(void);
 
-#ifndef GUIDANCE_INDI_THRUST_DYNAMICS
+#ifdef GUIDANCE_INDI_THRUST_DYNAMICS
+#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."
+#endif
+
+#ifndef GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
 #ifndef STABILIZATION_INDI_ACT_DYN_P
 #error "You need to define GUIDANCE_INDI_THRUST_DYNAMICS 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 STABILIZATION_INDI_ACT_DYN_P
+#define GUIDANCE_INDI_THRUST_DYNAMICS_FREQ STABILIZATION_INDI_ACT_DYN_P
 #endif
-#endif //GUIDANCE_INDI_THRUST_DYNAMICS
+#endif //GUIDANCE_INDI_THRUST_DYNAMICS_FREQ
 
 #endif //GUIDANCE_INDI_SPECIFIC_FORCE_GAIN
 
@@ -161,6 +168,7 @@ float guidance_indi_min_pitch = GUIDANCE_INDI_MIN_PITCH;
 /** state eulers in zxy order */
 struct FloatEulers eulers_zxy;
 
+float thrust_dyn = 0.f;
 float thrust_act = 0;
 Butterworth2LowPass filt_accel_ned[3];
 Butterworth2LowPass roll_filt;
@@ -285,6 +293,12 @@ void guidance_indi_init(void)
   /*AbiBindMsgACCEL_SP(GUIDANCE_INDI_ACCEL_SP_ID, &accel_sp_ev, accel_sp_cb);*/
   AbiBindMsgVEL_SP(GUIDANCE_INDI_VEL_SP_ID, &vel_sp_ev, vel_sp_cb);
 
+#ifdef GUIDANCE_INDI_THRUST_DYNAMICS
+  thrust_dyn = GUIDANCE_INDI_THRUST_DYNAMICS;
+#else
+  thrust_dyn = 1-exp(-GUIDANCE_INDI_THRUST_DYNAMICS_FREQ/PERIODIC_FREQUENCY);
+#endif
+
   float tau = 1.0/(2.0*M_PI*filter_cutoff);
   float sample_time = 1.0/PERIODIC_FREQUENCY;
   for(int8_t i=0; i<3; i++) {
@@ -736,7 +750,7 @@ struct StabilizationSetpoint guidance_indi_run_mode(bool in_flight UNUSED, struc
 void guidance_indi_filter_thrust(void)
 {
   // Actuator dynamics
-  thrust_act = thrust_act + GUIDANCE_INDI_THRUST_DYNAMICS * (thrust_in - thrust_act);
+  thrust_act = thrust_act + thrust_dyn * (thrust_in - thrust_act);
 
   // same filter as for the acceleration
   update_butterworth_2_low_pass(&thrust_filt, thrust_act);

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c

@@ -167,7 +167,15 @@ float act_pref[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_PREF;
 float act_pref[INDI_NUM_ACT] = {0.0};
 #endif
 
-float act_dyn[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
+#ifdef STABILIZATION_INDI_ACT_DYN
+#warning STABILIZATION_INDI_ACT_DYN is deprecated, use STABILIZATION_INDI_ACT_FREQ instead.
+#warning You now have to define the continuous time corner frequency in rad/s of the actuators.
+#warning "Use -log(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old values.
+float act_first_order_cutoff[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_FREQ;
+float act_dyn_discrete[INDI_NUM_ACT];
+#else
+float act_dyn_discrete[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
+#endif
 
 #ifdef STABILIZATION_INDI_WLS_PRIORITIES
 static float Wv[INDI_OUTPUTS] = STABILIZATION_INDI_WLS_PRIORITIES;
@@ -284,7 +292,7 @@ void sum_g1_g2(void);
 static void send_eff_mat_g_indi(struct transport_tx *trans, struct link_device *dev)
 {
   float zero = 0.0;
-  pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID, 
+  pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID,
                                    1, &zero,
                                    1, &zero,
                                    1, &zero,
@@ -341,7 +349,15 @@ void stabilization_indi_init(void)
   // Initialize filters
   init_filters();
 
-#if STABILIZATION_INDI_RPM_FEEDBACK 
+#ifdef STABILIZATION_INDI_ACT_FREQ
+  int8_t i;
+  // Initialize the array of pointers to the rows of g1g2
+  for (i = 0; i < INDI_NUM_ACT; i++) {
+    act_dyn_discrete[i] = 1-exp(-act_first_order_cutoff[i]/PERIODIC_FREQUENCY);
+  }
+#endif
+
+#if STABILIZATION_INDI_RPM_FEEDBACK
   AbiBindMsgACT_FEEDBACK(STABILIZATION_INDI_ACT_FEEDBACK_ID, &act_feedback_ev, act_feedback_cb);
 #endif
   AbiBindMsgTHRUST(THRUST_INCREMENT_ID, &thrust_ev, thrust_cb);
@@ -361,7 +377,6 @@ void stabilization_indi_init(void)
   calc_g1g2_pseudo_inv();
 #endif
 
-  int8_t i;
   // Initialize the array of pointers to the rows of g1g2
   for (i = 0; i < INDI_OUTPUTS; i++) {
     Bwls[i] = g1g2[i];
@@ -833,7 +848,7 @@ void get_actuator_state(void)
     prev_actuator_state = actuator_state[i];
 
     actuator_state[i] = actuator_state[i]
-                        + act_dyn[i] * (indi_u[i] - actuator_state[i]);
+                        + act_dyn_discrete[i] * (indi_u[i] - actuator_state[i]);
 
 #ifdef STABILIZATION_INDI_ACT_RATE_LIMIT
     if ((actuator_state[i] - prev_actuator_state) > act_rate_limit[i]) {

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c

@@ -41,8 +41,13 @@
 #include "modules/radio_control/radio_control.h"
 #include "filters/low_pass_filter.h"
 
-#if !defined(STABILIZATION_INDI_ACT_DYN_P) && !defined(STABILIZATION_INDI_ACT_DYN_Q) && !defined(STABILIZATION_INDI_ACT_DYN_R)
-#error You have to define the first order time constant of the actuator dynamics!
+#if defined(STABILIZATION_INDI_ACT_DYN_P) && !defined(STABILIZATION_INDI_ACT_DYN_Q) && !defined(STABILIZATION_INDI_ACT_DYN_R)
+#warning STABILIZATION_INDI_ACT_DYN is deprecated, use STABILIZATION_INDI_ACT_FREQ instead.
+#warning You now have to define the continuous time corner frequency in rad/s of the actuators.
+#warning "Use -log(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old values.
+#else
+#if !defined(STABILIZATION_INDI_ACT_FREQ_P) && !defined(STABILIZATION_INDI_ACT_FREQ_Q) && !defined(STABILIZATION_INDI_ACT_FREQ_R)
+#warning You have to define the corner frequency of the first order actuator dynamics model in rad/s!
 #endif
 
 // these parameters are used in the filtering of the angular acceleration
@@ -169,13 +174,13 @@ static void send_eff_mat_g_indi_simple(struct transport_tx *trans, struct link_d
   RATES_SMUL(g1_disp, indi.est.g1, INDI_EST_SCALE);
   float g2_disp = indi.est.g2 * INDI_EST_SCALE;
   float zero = 0.0;
-  pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID, 
+  pprz_msg_send_EFF_MAT_G(trans, dev, AC_ID,
                                     1, &zero,
                                     1, &zero,
                                     1, &zero,
                                     1, &g1_disp.p,
                                     1, &g1_disp.q,
-                                    1, &g1_disp.r, 
+                                    1, &g1_disp.r,
                                     1, &g2_disp,
                                     1, &zero);
 }
@@ -200,6 +205,16 @@ void stabilization_indi_init(void)
   // Initialize filters
   indi_init_filters();
 
+#ifdef STABILIZATION_INDI_ACT_FREQ_P
+  indi.act_dyn.p = 1-exp(-STABILIZATION_INDI_ACT_FREQ_P/PERIODIC_FREQUENCY);
+  indi.act_dyn.q = 1-exp(-STABILIZATION_INDI_ACT_FREQ_Q/PERIODIC_FREQUENCY);
+  indi.act_dyn.r = 1-exp(-STABILIZATION_INDI_ACT_FREQ_R/PERIODIC_FREQUENCY);
+#else
+  indi.act_dyn.p = STABILIZATION_INDI_ACT_DYN_P;
+  indi.act_dyn.q = STABILIZATION_INDI_ACT_DYN_Q;
+  indi.act_dyn.r = STABILIZATION_INDI_ACT_DYN_R;
+#endif
+
 #if PERIODIC_TELEMETRY
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_STAB_ATTITUDE, send_att_indi);
   register_periodic_telemetry(DefaultPeriodic, PPRZ_MSG_ID_EFF_MAT_G, send_eff_mat_g_indi_simple);
@@ -374,9 +389,9 @@ void stabilization_indi_rate_run(struct FloatRates rate_sp, bool in_flight __att
 {
   //Propagate input filters
   //first order actuator dynamics
-  indi.u_act_dyn.p = indi.u_act_dyn.p + STABILIZATION_INDI_ACT_DYN_P * (indi.u_in.p - indi.u_act_dyn.p);
-  indi.u_act_dyn.q = indi.u_act_dyn.q + STABILIZATION_INDI_ACT_DYN_Q * (indi.u_in.q - indi.u_act_dyn.q);
-  indi.u_act_dyn.r = indi.u_act_dyn.r + STABILIZATION_INDI_ACT_DYN_R * (indi.u_in.r - indi.u_act_dyn.r);
+  indi.u_act_dyn.p = indi.u_act_dyn.p + indi.act_dyn.p * (indi.u_in.p - indi.u_act_dyn.p);
+  indi.u_act_dyn.q = indi.u_act_dyn.q + indi.act_dyn.q * (indi.u_in.q - indi.u_act_dyn.q);
+  indi.u_act_dyn.r = indi.u_act_dyn.r + indi.act_dyn.r * (indi.u_in.r - indi.u_act_dyn.r);
 
   // Propagate the filter on the gyroscopes and actuators
   struct FloatRates *body_rates = stateGetBodyRates_f();

sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.h

@@ -70,6 +70,7 @@ struct IndiVariables {
   Butterworth2LowPass rate[3];
   struct FloatRates g1;
   float g2;
+  struct FloatRates act_dyn;
 
   struct Indi_gains gains;