actuators becomes an array

paparazzi · Dec 17, 2023 · f461e8c · f461e8c
1 parent d2edf55
commit f461e8c
Show file tree

Hide file tree

Showing 5 changed files with 28 additions and 15 deletions.
diff --git 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
@@ -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
@@ -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
@@ -72,7 +79,7 @@ extern int16_t actuators_pprz[ACTUATORS_NB];
  * @param _n actuators name as given in airframe file, servos section
  * @param _v new actuator's value
  */
-#define _ActuatorSet(_n, _v) Set_ ## _n ## _Servo(_v)
+#define _ActuatorSet(_n, _v) Set_ ## _n ## _Servo(_v,_v)
 #define ActuatorSet(_n, _v) _ActuatorSet(_n, _v)
 
 #endif /* ACTUATORS_NB */

diff --git 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/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml
@@ -210,8 +210,9 @@ let print_actuators_idx = fun out ->
     fprintf out "#define SERVO_%s_IDX %d\n" s i;
     (* Set servo macro *)
     fprintf out "#define Set_%s_Servo(_pprzv,_s) { \\\n" s;
-    fprintf out "  actuators[SERVO_%s_IDX] = _pprzv; \\\n" s;
-    fprintf out "  Actuator%sSet(SERVO_%s_DRIVER_NO, Clip(_s, SERVO_%s_MIN, SERVO_%s_MAX)); \\\n" d s s s;
+    fprintf out "  actuators[SERVO_%s_IDX].pprz_val = ClipAbs( _pprzv, MAX_PPRZ); \\\n" s;
+    fprintf out "  actuators[SERVO_%s_IDX].driver_val = Clip(_s, 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));