From 979c01cd7c8efe1239d7ffa5a23783cf2bb6ea9b Mon Sep 17 00:00:00 2001
From: Gautier Hattenberger <gautier.hattenberger@enac.fr>
Date: Thu, 19 Feb 2026 10:46:12 +0100
Subject: [PATCH] [stabilization] set commands from indi actuators array
 (#3606)

Make it possible to set the command vector from the stabilization INDI controller, when STABILIZATION_INDI_COMMANDS array is defined. The benefit is a simpler configuration of the control_laws section of the airframe file when using INDI.
Be careful that with this, a "correct" failsafe_value is needed for the commands, since it is the value applied in KILL mode. Some special keywords (e.g. MOTOR_STOP) are added to help avoiding mistakes.

Backward compatible if STABILIZATION_INDI_COMMANDS is not defined.

- add example airframe
- add special names for command failsafe values:
  - MOTOR_STOP = MIN_PPRZ = -9600
  - MAX_PPRZ = 9600
  - MOTOR_IDLE = 0
---
 .../ENAC/quadrotor/anton_indi_aruco.xml       | 18 +++++++----
 .../stabilization/stabilization_indi.c        | 32 +++++++++++++++++++
 sw/ext/opencv_bebop                           |  2 +-
 sw/tools/generators/gen_airframe.ml           |  7 +++-
 4 files changed, 50 insertions(+), 9 deletions(-)

diff --git a/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml b/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
index ba8b7e4b80..7a4eafd68d 100644
--- a/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
+++ b/conf/airframes/ENAC/quadrotor/anton_indi_aruco.xml
@@ -101,17 +101,18 @@
   </servos>
 
   <commands>
-    <axis name="ROLL" failsafe_value="0"/>
-    <axis name="PITCH" failsafe_value="0"/>
-    <axis name="YAW" failsafe_value="0"/>
+    <axis name="FR" failsafe_value="MOTOR_STOP"/>
+    <axis name="BR" failsafe_value="MOTOR_STOP"/>
+    <axis name="BL" failsafe_value="MOTOR_STOP"/>
+    <axis name="FL" failsafe_value="MOTOR_STOP"/>
     <axis name="THRUST" failsafe_value="0"/>
   </commands>
 
   <command_laws>
-    <set servo="FR" value="autopilot_get_motors_on() ? actuators_pprz[0] : -MAX_PPRZ"/>
-    <set servo="BR" value="autopilot_get_motors_on() ? actuators_pprz[1] : -MAX_PPRZ"/>
-    <set servo="BL" value="autopilot_get_motors_on() ? actuators_pprz[2] : -MAX_PPRZ"/>
-    <set servo="FL" value="autopilot_get_motors_on() ? actuators_pprz[3] : -MAX_PPRZ"/>
+    <set servo="FR" value="@FR"/>
+    <set servo="BR" value="@BR"/>
+    <set servo="BL" value="@BL"/>
+    <set servo="FL" value="@FL"/>
   </command_laws>
 
   <section name="IMU" prefix="IMU_">
@@ -196,6 +197,9 @@
 
     <!--Priority for each axis (roll, pitch, yaw and thrust)-->
     <define name="WLS_PRIORITIES" value="{1000, 1000, 1, 100}"/>
+
+    <!-- INDI outputs to commands association -->
+    <define name="COMMANDS" value="{COMMAND_FR, COMMAND_BR, COMMAND_BL, COMMAND_FL}"/>
   </section>
 
   <section name="GUIDANCE_INDI" prefix="GUIDANCE_INDI_">
diff --git a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
index b4f31b9361..bee83175ba 100644
--- a/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
+++ b/sw/airborne/firmwares/rotorcraft/stabilization/stabilization_indi.c
@@ -220,6 +220,10 @@ static bool act_thrust_mat[3][INDI_NUM_ACT] = {
 };
 #endif
 
+#ifdef STABILIZATION_INDI_COMMANDS
+static uint8_t act_to_commands[INDI_NUM_ACT] = STABILIZATION_INDI_COMMANDS;
+#endif
+
 #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.
@@ -773,6 +777,10 @@ void stabilization_indi_rate_run(bool in_flight, struct StabilizationSetpoint *s
 
     // commit actuator command
     actuators_pprz[i] = (int16_t) indi_u[i];
+#ifdef STABILIZATION_INDI_COMMANDS
+    // copy to actuators to specified commands
+    cmd[act_to_commands[i]] = actuators_pprz[i];
+#endif
 
     // update thrust command such that the current is correctly estimated
     cmd[COMMAND_THRUST] += actuator_state[i] * (int32_t) act_thrust_mat[2][i];
@@ -915,6 +923,30 @@ void get_actuator_state(void)
 #endif
 }
 
+#ifdef STABILIZATION_INDI_COMMANDS
+/** Redefine if actuators to commands mapping is defined
+ *
+ * copy stabilization commands to general commands
+ * if corresponding actuator axis is producing thrust, stop motor is needed
+ */
+void set_rotorcraft_commands(pprz_t *cmd_out, int32_t *cmd_in, bool in_flight __attribute__((unused)), bool motors_on)
+{
+  for (int i = 0; i < INDI_NUM_ACT; i++) {
+    bool is_motor = act_thrust_mat[0][i] || act_thrust_mat[1][i] || act_thrust_mat[2][i];
+    if (is_motor && !motors_on) {
+      cmd_out[act_to_commands[i]] = MIN_PPRZ;
+    } else {
+      cmd_out[act_to_commands[i]] = cmd_in[act_to_commands[i]];
+    }
+  }
+  // for GCS display
+  if (!motors_on) {
+    cmd_in[COMMAND_THRUST] = 0;
+  }
+  cmd_out[COMMAND_THRUST] = cmd_in[COMMAND_THRUST];
+}
+#endif
+
 /**
  * @param ddx_error error in output change
  * @param i row of the matrix element
diff --git a/sw/ext/opencv_bebop b/sw/ext/opencv_bebop
index 472e91798b..b2264ae3ee 160000
--- a/sw/ext/opencv_bebop
+++ b/sw/ext/opencv_bebop
@@ -1 +1 @@
-Subproject commit 472e91798b339784e404eb622d9343e00c07a432
+Subproject commit b2264ae3ee4bc8e79df4d66eff23e8fb3f7ab557
diff --git a/sw/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml
index 30bd909207..b48a7a2fbc 100644
--- a/sw/tools/generators/gen_airframe.ml
+++ b/sw/tools/generators/gen_airframe.ml
@@ -349,7 +349,12 @@ let parse_ap_only_commands = fun out ap_only ->
 
 let parse_command = fun out command no ->
   let command_name = "COMMAND_"^ExtXml.attrib command "name" in
-  let failsafe_value = int_of_string (ExtXml.attrib command "failsafe_value") in
+  let failsafe_value = match (ExtXml.attrib command "failsafe_value") with
+    | "MIN_PPRZ" | "MOTOR_STOP" -> -9600
+    | "MAX_PPRZ" -> 9600
+    | "MOTOR_IDLE" -> 0
+    | x -> int_of_string x
+  in
   let group = ExtXml.attrib_or_default command "group" "REAL" in 
   define_out out command_name (string_of_int no);
   { failsafe_value = failsafe_value; foo = 0 ; group = group }