From 2083b5898f84dad58a602cfcea6508cd10dae5f2 Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Thu, 7 Dec 2023 20:19:08 +0100
Subject: [PATCH] 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
---
 .../firmwares/rotorcraft/guidance/guidance_indi_hybrid.c | 2 ++
 .../rotorcraft/stabilization/stabilization_indi.c        | 9 +++++----
 .../rotorcraft/stabilization/stabilization_indi_simple.c | 2 +-
 3 files changed, 8 insertions(+), 5 deletions(-)

diff --git a/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c b/sw/airborne/firmwares/rotorcraft/guidance/guidance_indi_hybrid.c
index 109bd6c756..9f91fbcbc8 100644
--- 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
index 9daa2c8c8f..f8a2b40f86 100644
--- 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
index 37317d7e12..6d59875a6a 100644
--- 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!