Fix continuous actuator (#3196)

* fix extra bug for #3190 * fix extra extra bug for #3190 * fix for fix of fix * final fix? * Update comment: specify natural log = ln
paparazzi · Dec 7, 2023 · 2083b58 · 2083b58
1 parent 7279d9b
commit 2083b58
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 5 deletions.
diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
@@ -293,10 +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_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
 
   float tau = 1.0/(2.0*M_PI*filter_cutoff);

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -170,11 +170,11 @@ float act_pref[INDI_NUM_ACT] = {0.0};
 #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_DYN;
-float act_dyn_discrete[INDI_NUM_ACT];
+#warning "Use -ln(1 - old_number) * PERIODIC_FREQUENCY to compute it from the old values."
+float act_dyn_discrete[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_DYN;
 #else
-float act_dyn_discrete[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_FREQ;
+float act_first_order_cutoff[INDI_NUM_ACT] = STABILIZATION_INDI_ACT_FREQ;
+float act_dyn_discrete[INDI_NUM_ACT]; // will be computed from freq at init
 #endif
 
 #ifdef STABILIZATION_INDI_WLS_PRIORITIES
@@ -350,6 +350,7 @@ void stabilization_indi_init(void)
   init_filters();
 
   int8_t i;
+// If the deprecated STABILIZATION_INDI_ACT_DYN is used, convert it to the new FREQUENCY format
 #ifdef STABILIZATION_INDI_ACT_FREQ
   // Initialize the array of pointers to the rows of g1g2
   for (i = 0; i < INDI_NUM_ACT; i++) {

diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi_simple.c
@@ -44,7 +44,7 @@
 #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."
+#warning "Use -ln(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!