From e6bcd60666fb5cf1369da19d9c58b9c1334b65ec Mon Sep 17 00:00:00 2001
From: Christophe De Wagter <dewagter@gmail.com>
Date: Sat, 25 May 2024 09:38:51 +0200
Subject: [PATCH] actuators[ ] array of structs with pprz units and driver
 units (#3205)

* actuators[ ] array in pprz units, conversion to actuator_driver units when sending to the actuator_driver.

actuators becomes an array

* Moved scaling of the Servo to the Set function.

* Fixed switches and pan-tilt to work in PPRZ units now. Simplified code.
---
 conf/airframes/CDW/cdw_mavtec.xml             |  4 ++--
 conf/airframes/ENAC/quadrotor/cobra.xml       |  2 --
 conf/airframes/ENAC/quadrotor/cobraV2.xml     |  4 ++--
 conf/airframes/ENAC/quadrotor/maya_indoor.xml |  2 --
 conf/airframes/OPENUAS/openuas_own_mavtec.xml |  2 --
 conf/airframes/tudelft/mavtec5.xml            |  4 ++--
 conf/modules/rotorcraft_cam.xml               |  2 +-
 conf/modules/switch_servo.xml                 |  8 ++++----
 conf/settings/setup_actuators.xml             | 16 +++++++--------
 sw/airborne/modules/actuators/actuators.c     |  9 +++++++--
 sw/airborne/modules/actuators/actuators.h     |  9 ++++++++-
 .../modules/cam_control/rotorcraft_cam.c      | 20 +++++++------------
 .../modules/cam_control/rotorcraft_cam.h      |  2 +-
 .../loggers/logger_control_effectiveness.c    |  2 +-
 sw/airborne/modules/switching/switch_servo.h  |  4 ++--
 sw/tools/generators/gen_airframe.ml           | 18 +++++++++--------
 16 files changed, 55 insertions(+), 53 deletions(-)

diff --git a/conf/airframes/CDW/cdw_mavtec.xml b/conf/airframes/CDW/cdw_mavtec.xml
index b5596e9e0d..68da9adf0c 100644
--- a/conf/airframes/CDW/cdw_mavtec.xml
+++ b/conf/airframes/CDW/cdw_mavtec.xml
@@ -65,8 +65,8 @@
 
   <section name="SWITCH_SERVO">
     <define name="SWITCH_SERVO_SERVO" value="DROP"/>
-    <define name="SWITCH_SERVO_ON_VALUE" value="SERVO_DROP_MIN"/>
-    <define name="SWITCH_SERVO_OFF_VALUE" value="SERVO_DROP_MAX"/>
+    <define name="SWITCH_SERVO_ON_VALUE" value="MIN_PPRZ"/>
+    <define name="SWITCH_SERVO_OFF_VALUE" value="MAX_PPRZ"/>
     <define name="DropOpen()" value="SwitchServoOn()"/>
     <define name="DropClose()" value="SwitchServoOff()"/>
   </section>
diff --git a/conf/airframes/ENAC/quadrotor/cobra.xml b/conf/airframes/ENAC/quadrotor/cobra.xml
index f208865dfb..fa313485c4 100644
--- a/conf/airframes/ENAC/quadrotor/cobra.xml
+++ b/conf/airframes/ENAC/quadrotor/cobra.xml
@@ -34,8 +34,6 @@
     <module name="actuators" type="dshot"/>
     <module name="actuators" type="pwm"/>
     <module name="switch" type="servo"/>
-    <define name="SWITCH_SERVO_ON_VALUE" value="SERVO_SWITCH_MAX"/>
-    <define name="SWITCH_SERVO_OFF_VALUE" value="SERVO_SWITCH_MIN"/>
 
     <module name="board" type="tawaki">
       <define name="IMU_MPU_LOWPASS_FILTER" value="MPU60X0_DLPF_256HZ"/>
diff --git a/conf/airframes/ENAC/quadrotor/cobraV2.xml b/conf/airframes/ENAC/quadrotor/cobraV2.xml
index 29c7d86f9e..979bfa2f98 100644
--- a/conf/airframes/ENAC/quadrotor/cobraV2.xml
+++ b/conf/airframes/ENAC/quadrotor/cobraV2.xml
@@ -35,8 +35,8 @@
     </module>
     <module name="actuators" type="pwm"/>
     <module name="switch" type="servo"/>
-    <define name="SWITCH_SERVO_ON_VALUE" value="SERVO_SWITCH_MAX"/>
-    <define name="SWITCH_SERVO_OFF_VALUE" value="SERVO_SWITCH_MIN"/>
+    <define name="SWITCH_SERVO_ON_VALUE" value="MAX_PPRZ"/>
+    <define name="SWITCH_SERVO_OFF_VALUE" value="MIN_PPRZ"/>
 
     <module name="board" type="tawaki_2.0">
     </module>
diff --git a/conf/airframes/ENAC/quadrotor/maya_indoor.xml b/conf/airframes/ENAC/quadrotor/maya_indoor.xml
index 34d07b164c..e4c6576d21 100644
--- a/conf/airframes/ENAC/quadrotor/maya_indoor.xml
+++ b/conf/airframes/ENAC/quadrotor/maya_indoor.xml
@@ -30,8 +30,6 @@
     <module name="actuators" type="dshot"/>
     <module name="actuators" type="pwm"/>
     <module name="switch" type="servo"/>
-    <define name="SWITCH_SERVO_ON_VALUE" value="SERVO_SWITCH_MAX"/>
-    <define name="SWITCH_SERVO_OFF_VALUE" value="SERVO_SWITCH_MIN"/>
 
     <module name="board" type="tawaki">
       <define name="IMU_MPU_LOWPASS_FILTER" value="MPU60X0_DLPF_256HZ"/>
diff --git a/conf/airframes/OPENUAS/openuas_own_mavtec.xml b/conf/airframes/OPENUAS/openuas_own_mavtec.xml
index 2a5d4a5cee..12fd6e5988 100644
--- a/conf/airframes/OPENUAS/openuas_own_mavtec.xml
+++ b/conf/airframes/OPENUAS/openuas_own_mavtec.xml
@@ -62,8 +62,6 @@
 
   <section name="SWITCH_SERVO">
     <define name="SWITCH_SERVO_SERVO" value="DROP"/>
-    <define name="SWITCH_SERVO_ON_VALUE" value="SERVO_DROP_MIN"/>
-    <define name="SWITCH_SERVO_OFF_VALUE" value="SERVO_DROP_MAX"/>
     <define name="DropOpen()" value="SwitchServoOn()"/>
     <define name="DropClose()" value="SwitchServoOff()"/>
   </section>
diff --git a/conf/airframes/tudelft/mavtec5.xml b/conf/airframes/tudelft/mavtec5.xml
index 8084871176..29bee86f16 100644
--- a/conf/airframes/tudelft/mavtec5.xml
+++ b/conf/airframes/tudelft/mavtec5.xml
@@ -74,8 +74,8 @@
 
   <section name="SWITCH_SERVO">
     <define name="SWITCH_SERVO_SERVO" value="DROP"/>
-    <define name="SWITCH_SERVO_ON_VALUE" value="SERVO_DROP_MIN"/>
-    <define name="SWITCH_SERVO_OFF_VALUE" value="SERVO_DROP_MAX"/>
+    <define name="SWITCH_SERVO_ON_VALUE" value="MIN_PPRZ"/>
+    <define name="SWITCH_SERVO_OFF_VALUE" value="MAX_PPRZ"/>
     <define name="DropOpen()" value="SwitchServoOn()"/>
     <define name="DropClose()" value="SwitchServoOff()"/>
   </section>
diff --git a/conf/modules/rotorcraft_cam.xml b/conf/modules/rotorcraft_cam.xml
index 811fa5b3b5..608083c7c4 100644
--- a/conf/modules/rotorcraft_cam.xml
+++ b/conf/modules/rotorcraft_cam.xml
@@ -42,7 +42,7 @@ On boards with CAM_SWITCH, ROTORCRAFT_CAM_SWITCH_GPIO can be defined to CAM_SWIT
           <key_press key="F1" value="0"/>
           <key_press key="F2" value="1"/>
         </dl_setting>
-        <dl_setting var="rotorcraft_cam_tilt_pwm" min="1000" step="1" max="2500" shortname="tilt_pwm"/>
+        <dl_setting var="rotorcraft_cam_tilt_pprz" min="MIN_PPRZ" step="1" max="MAX_PPRZ" shortname="tilt_pprz"/>
         <dl_setting var="rotorcraft_cam_tilt" min="-90" step="1" max="0" shortname="tilt" alt_unit="deg" alt_unit_coef="0.0139882">
           <strip_button name="Look Foreward" icon="lookfore.png" value="0" group="cam_look"/>
           <strip_button name="Look Down" icon="lookdown.png" value="-6434" group="cam_look"/>
diff --git a/conf/modules/switch_servo.xml b/conf/modules/switch_servo.xml
index 77c57dc964..c6a25fd202 100644
--- a/conf/modules/switch_servo.xml
+++ b/conf/modules/switch_servo.xml
@@ -3,8 +3,8 @@
 <module name="switch_servo" dir="switching">
   <doc>
     <description>Swicht using a Servo</description>
-    <define name="SWITCH_SERVO_ON_VALUE"  value="pwm" description="servo value in usec"/>
-    <define name="SWITCH_SERVO_OFF_VALUE" value="pwm" description="servo value in usec"/>
+    <define name="SWITCH_SERVO_ON_VALUE"  value="pprz_cmd" description="servo value in pprz units"/>
+    <define name="SWITCH_SERVO_OFF_VALUE" value="pprz_cmd" description="servo value in pprz units"/>
     <define name="SWITCH_SERVO_SERVO"     value="nb" description="Id of the servo (default: SWITCH)"/>
   </doc>
   <settings>
@@ -27,8 +27,8 @@
 <!-- these parameters should be set for that module in the airframe file unless you want the defaults
      Servo value in usec
   <load name="switch_servo.xml">
-    <define name="SWITCH_SERVO_ON_VALUE"  value="2000"/>
-    <define name="SWITCH_SERVO_OFF_VALUE" value="1000"/>
+    <define name="SWITCH_SERVO_ON_VALUE"  value="MAX_PPRZ"/>
+    <define name="SWITCH_SERVO_OFF_VALUE" value="MIN_PPRZ"/>
     <define name="SWITCH_SERVO_SERVO"     value="SWITCH"/>
   </load>
 
diff --git a/conf/settings/setup_actuators.xml b/conf/settings/setup_actuators.xml
index 8719a1a0b5..ad50895efb 100644
--- a/conf/settings/setup_actuators.xml
+++ b/conf/settings/setup_actuators.xml
@@ -5,14 +5,14 @@
 <settings target="setup_actuators">
   <dl_settings>
     <dl_settings NAME="Actuators" >
-      <dl_setting VAR="actuators[0]" MIN="900" STEP="1" MAX="2100" module="modules/actuators/actuators" shortname="chan0"/>
-      <dl_setting VAR="actuators[1]" MIN="900" STEP="1" MAX="2100" shortname="chan1"/>
-      <dl_setting VAR="actuators[2]" MIN="900" STEP="1" MAX="2100" shortname="chan2"/>
-      <dl_setting VAR="actuators[3]" MIN="900" STEP="1" MAX="2100" shortname="chan3"/>
-      <dl_setting VAR="actuators[4]" MIN="900" STEP="1" MAX="2100" shortname="chan4"/>
-      <dl_setting VAR="actuators[5]" MIN="900" STEP="1" MAX="2100" shortname="chan5"/>
-      <dl_setting VAR="actuators[6]" MIN="900" STEP="1" MAX="2100" shortname="chan6"/>
-      <dl_setting VAR="actuators[7]" MIN="900" STEP="1" MAX="2100" shortname="chan7"/>
+      <dl_setting VAR="actuators[0].driver_val" MIN="900" STEP="1" MAX="2100" module="modules/actuators/actuators" shortname="chan0"/>
+      <dl_setting VAR="actuators[1].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan1"/>
+      <dl_setting VAR="actuators[2].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan2"/>
+      <dl_setting VAR="actuators[3].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan3"/>
+      <dl_setting VAR="actuators[4].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan4"/>
+      <dl_setting VAR="actuators[5].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan5"/>
+      <dl_setting VAR="actuators[6].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan6"/>
+      <dl_setting VAR="actuators[7].driver_val" MIN="900" STEP="1" MAX="2100" shortname="chan7"/>
     </dl_settings>
   </dl_settings>
 </settings>
diff --git a/sw/airborne/modules/actuators/actuators.c b/sw/airborne/modules/actuators/actuators.c
index 9595c836b1..4ae17dd3cb 100644
--- a/sw/airborne/modules/actuators/actuators.c
+++ b/sw/airborne/modules/actuators/actuators.c
@@ -42,11 +42,16 @@
 
 static void send_actuators(struct transport_tx *trans, struct link_device *dev)
 {
-  pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, actuators);
+  // Downlink the actuators raw driver values
+  int16_t v[ACTUATORS_NB] = {0};
+  for (int i = 0; i < ACTUATORS_NB; i++) {
+    v[i] = actuators[i].driver_val;
+  }
+  pprz_msg_send_ACTUATORS(trans, dev, AC_ID , ACTUATORS_NB, v);
 }
 #endif
 
-int16_t actuators[ACTUATORS_NB];
+struct actuator_t actuators[ACTUATORS_NB];
 
 // Can be used to directly control each actuator from the control algorithm
 int16_t actuators_pprz[ACTUATORS_NB];
diff --git a/sw/airborne/modules/actuators/actuators.h b/sw/airborne/modules/actuators/actuators.h
index 929e552d5e..718e600a35 100644
--- a/sw/airborne/modules/actuators/actuators.h
+++ b/sw/airborne/modules/actuators/actuators.h
@@ -57,10 +57,17 @@ struct act_feedback_t {
 extern uint32_t actuators_delay_time;
 extern bool   actuators_delay_done;
 
+// Actuator feedback structure for ABI Message
+struct actuator_t {
+  pprz_t pprz_val;                  ///< Actuator value in PPRZ units
+  int16_t driver_val;               ///< Actuator value in driver units (scaling from servo in airframe.h)
+};
+
+
 /** Actuators array.
  * Temporary storage (for debugging purpose, downlinked via telemetry)
  * */
-extern int16_t actuators[ACTUATORS_NB];
+extern struct actuator_t actuators[ACTUATORS_NB];
 
 /** PPRZ command to each actuator
  * Can be used to directly control actuators from the control algorithm
diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.c b/sw/airborne/modules/cam_control/rotorcraft_cam.c
index 08623234f8..21efcf3baa 100644
--- a/sw/airborne/modules/cam_control/rotorcraft_cam.c
+++ b/sw/airborne/modules/cam_control/rotorcraft_cam.c
@@ -73,12 +73,8 @@ uint8_t rotorcraft_cam_mode;
 
 // Tilt definition
 int16_t rotorcraft_cam_tilt;
-int16_t rotorcraft_cam_tilt_pwm;
+int16_t rotorcraft_cam_tilt_pprz;
 #if ROTORCRAFT_CAM_USE_TILT
-#define ROTORCRAFT_CAM_TILT_NEUTRAL SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, NEUTRAL)
-#define ROTORCRAFT_CAM_TILT_MIN SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, MIN)
-#define ROTORCRAFT_CAM_TILT_MAX SERVO_PARAM(ROTORCRAFT_CAM_TILT_SERVO, MAX)
-#define D_TILT (ROTORCRAFT_CAM_TILT_MAX - ROTORCRAFT_CAM_TILT_MIN)
 #define CT_MIN Min(CAM_TA_MIN, CAM_TA_MAX)
 #define CT_MAX Max(CAM_TA_MIN, CAM_TA_MAX)
 #endif
@@ -112,11 +108,9 @@ void rotorcraft_cam_init(void)
   gpio_setup_output(ROTORCRAFT_CAM_SWITCH_GPIO);
 #endif
   rotorcraft_cam_set_mode(ROTORCRAFT_CAM_DEFAULT_MODE);
+  rotorcraft_cam_tilt_pprz = 0;
 #if ROTORCRAFT_CAM_USE_TILT
-  rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
-  ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
-#else
-  rotorcraft_cam_tilt_pwm = 1500;
+  ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pprz);
 #endif
   rotorcraft_cam_tilt = 0;
   rotorcraft_cam_pan = 0;
@@ -130,7 +124,7 @@ void rotorcraft_cam_periodic(void)
   switch (rotorcraft_cam_mode) {
     case ROTORCRAFT_CAM_MODE_NONE:
 #if ROTORCRAFT_CAM_USE_TILT
-      rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_NEUTRAL;
+      rotorcraft_cam_tilt_pprz = 0;
 #endif
 #if ROTORCRAFT_CAM_USE_PAN
       rotorcraft_cam_pan = stateGetNedToBodyEulers_i()->psi;
@@ -142,7 +136,7 @@ void rotorcraft_cam_periodic(void)
     case ROTORCRAFT_CAM_MODE_HEADING:
 #if ROTORCRAFT_CAM_USE_TILT_ANGLES
       Bound(rotorcraft_cam_tilt, CT_MIN, CT_MAX);
-      rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
+      rotorcraft_cam_tilt_pprz = MIN_PPRZ + 2 * MAX_PPRZ * (rotorcraft_cam_tilt - CAM_TA_MIN) /
                                 (CAM_TA_MAX - CAM_TA_MIN);
 #endif
 #if ROTORCRAFT_CAM_USE_PAN
@@ -164,7 +158,7 @@ void rotorcraft_cam_periodic(void)
         height = (waypoints[ROTORCRAFT_CAM_TRACK_WP].z - stateGetPositionEnu_i()->z) >> INT32_POS_FRAC;
         rotorcraft_cam_tilt = int32_atan2(height, dist);
         Bound(rotorcraft_cam_tilt, CAM_TA_MIN, CAM_TA_MAX);
-        rotorcraft_cam_tilt_pwm = ROTORCRAFT_CAM_TILT_MIN + D_TILT * (rotorcraft_cam_tilt - CAM_TA_MIN) /
+        rotorcraft_cam_tilt_pprz = MIN_PPRZ + 2 * MAX_PPRZ *  (rotorcraft_cam_tilt - CAM_TA_MIN) /
                                   (CAM_TA_MAX - CAM_TA_MIN);
 #endif
       }
@@ -174,6 +168,6 @@ void rotorcraft_cam_periodic(void)
       break;
   }
 #if ROTORCRAFT_CAM_USE_TILT
-  ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pwm);
+  ActuatorSet(ROTORCRAFT_CAM_TILT_SERVO, rotorcraft_cam_tilt_pprz);
 #endif
 }
diff --git a/sw/airborne/modules/cam_control/rotorcraft_cam.h b/sw/airborne/modules/cam_control/rotorcraft_cam.h
index b760f6c796..9bce22280a 100644
--- a/sw/airborne/modules/cam_control/rotorcraft_cam.h
+++ b/sw/airborne/modules/cam_control/rotorcraft_cam.h
@@ -92,7 +92,7 @@ extern uint8_t rotorcraft_cam_mode;
 
 extern int16_t rotorcraft_cam_tilt;
 extern int16_t rotorcraft_cam_pan;
-extern int16_t rotorcraft_cam_tilt_pwm;
+extern int16_t rotorcraft_cam_tilt_pprz;
 
 extern void rotorcraft_cam_init(void);
 extern void rotorcraft_cam_periodic(void);
diff --git a/sw/airborne/modules/loggers/logger_control_effectiveness.c b/sw/airborne/modules/loggers/logger_control_effectiveness.c
index 550326b969..e7bcdf243d 100644
--- a/sw/airborne/modules/loggers/logger_control_effectiveness.c
+++ b/sw/airborne/modules/loggers/logger_control_effectiveness.c
@@ -129,7 +129,7 @@ void logger_control_effectiveness_periodic(void)
   // log actuators
 #if LOGGER_CONTROL_EFFECTIVENESS_ACTUATORS
   for (unsigned int i = 0; i < ACTUATORS_NB; i++) {
-    sdLogWriteLog(pprzLogFile, ",%d", actuators[i]);
+    sdLogWriteLog(pprzLogFile, ",%d", actuators[i].pprz_val);
   }
 #endif
 
diff --git a/sw/airborne/modules/switching/switch_servo.h b/sw/airborne/modules/switching/switch_servo.h
index 909afb968c..fe465ebf6c 100644
--- a/sw/airborne/modules/switching/switch_servo.h
+++ b/sw/airborne/modules/switching/switch_servo.h
@@ -30,10 +30,10 @@ extern bool switch_servo_on;
 extern int16_t switch_servo_value;
 
 #ifndef SWITCH_SERVO_ON_VALUE
-#define SWITCH_SERVO_ON_VALUE 2000
+#define SWITCH_SERVO_ON_VALUE MAX_PPRZ
 #endif
 #ifndef SWITCH_SERVO_OFF_VALUE
-#define SWITCH_SERVO_OFF_VALUE 1000
+#define SWITCH_SERVO_OFF_VALUE MIN_PPRZ
 #endif
 #ifndef SWITCH_SERVO_SERVO
 #define SWITCH_SERVO_SERVO SWITCH
diff --git a/sw/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml
index 9af1995dc0..acd66c21b8 100644
--- a/sw/tools/generators/gen_airframe.ml
+++ b/sw/tools/generators/gen_airframe.ml
@@ -271,9 +271,13 @@ let preprocess_value = fun s v prefix ->
 let print_actuators_idx = fun out ->
   Hashtbl.iter (fun s (d, i) ->
     (* Set servo macro *)
-    fprintf out "#define Set_%s_Servo(_v) { \\\n" s;
-    fprintf out "  actuators[SERVO_%s_IDX] = Clip(_v, SERVO_%s_MIN, SERVO_%s_MAX); \\\n" s s s;
-    fprintf out "  Actuator%sSet(SERVO_%s_DRIVER_NO, actuators[SERVO_%s_IDX]); \\\n" d s s;
+    fprintf out "#define Set_%s_Servo(actuator_value_pprz) { \\\n" s;
+    fprintf out "  actuators[SERVO_%s_IDX].pprz_val = ClipAbs( actuator_value_pprz, MAX_PPRZ); \\\n" s;
+    fprintf out "  command_value = actuator_value_pprz * (actuator_value_pprz>0 ? SERVO_%s_TRAVEL_UP_NUM : SERVO_%s_TRAVEL_DOWN_NUM); \\\n" s s;
+    fprintf out "  command_value /= actuator_value_pprz>0 ? SERVO_%s_TRAVEL_UP_DEN : SERVO_%s_TRAVEL_DOWN_DEN; \\\n" s s;
+    fprintf out "  servo_value = SERVO_%s_NEUTRAL + command_value; \\\n" s;
+    fprintf out "  actuators[SERVO_%s_IDX].driver_val = Clip(servo_value, SERVO_%s_MIN, SERVO_%s_MAX); \\\n" s s s;
+    fprintf out "  Actuator%sSet(SERVO_%s_DRIVER_NO, actuators[SERVO_%s_IDX].driver_val); \\\n" d s s;
     fprintf out "}\n\n"
   ) servos_drivers;
   define_out out "ACTUATORS_NB" (string_of_int (Hashtbl.length servos_drivers));
@@ -286,11 +290,8 @@ let parse_command_laws = fun out command ->
         let servo = a "servo"
         and value = a "value" in
         let v = preprocess_value value "values" "COMMAND" in
-        fprintf out "  command_value = %s; \\\n" v;
-        fprintf out "  command_value *= command_value>0 ? SERVO_%s_TRAVEL_UP_NUM : SERVO_%s_TRAVEL_DOWN_NUM; \\\n" servo servo;
-        fprintf out "  command_value /= command_value>0 ? SERVO_%s_TRAVEL_UP_DEN : SERVO_%s_TRAVEL_DOWN_DEN; \\\n" servo servo;
-        fprintf out "  servo_value = SERVO_%s_NEUTRAL + command_value; \\\n" servo;
-        fprintf out "  Set_%s_Servo(servo_value); \\\n\\\n" servo
+        fprintf out "  actuator_value_pprz = %s; \\\n" v;
+        fprintf out "  Set_%s_Servo(actuator_value_pprz); \\\n\\\n" servo
     | "let" ->
       let var = a "var"
       and value = a "value" in
@@ -435,6 +436,7 @@ let rec parse_section = fun out ac_id s ->
       fprintf out "#define SetActuatorsFromCommands(values, AP_MODE) { \\\n";
       fprintf out "  int32_t servo_value;\\\n";
       fprintf out "  int32_t command_value;\\\n\\\n";
+      fprintf out "  int32_t actuator_value_pprz;\\\n\\\n";
       List.iter (parse_command_laws out) (Xml.children s);
       fprintf out "  AllActuatorsCommit(); \\\n";
       fprintf out "}\n\n";