Use Commands instead of actuator_pprz (#3265)

* Add actuator type attribute to command

* Implemented new command based counting of the atuators in the oneloop controller

* Added group attribute to dtd file. Initialize as empty subgroup count of commands. Avoid unnecessary array to list conversion.

* Generate list of names from defined commands

* Use the defined commands number and  name in NPS

* Update sw/tools/generators/gen_airframe.ml

Co-authored-by: Gautier Hattenberger <gautier.hattenberger@enac.fr>

* Added logic to remove redundant code

---------

Co-authored-by: Gautier Hattenberger <gautier.hattenberger@enac.fr>

conf/airframes/airframe.dtd

@@ -74,7 +74,8 @@ collective CDATA #REQUIRED>
 
 <!ATTLIST axis
 name CDATA #REQUIRED
-failsafe_value CDATA #REQUIRED>
+failsafe_value CDATA #REQUIRED
+group CDATA #IMPLIED>
 
 <!ATTLIST set
 value CDATA #REQUIRED

conf/modules/oneloop_andi.xml

@@ -28,13 +28,7 @@
   <init fun="oneloop_andi_init()"/>
    <makefile target="ap|nps" firmware="rotorcraft">
     <file       name="oneloop_andi.c" dir="$(SRC_FIRMWARE)/oneloop"/>
-    <configure  name="ANDI_OUTPUTS" default="4"/>
-    <configure  name="ANDI_NUM_ACT" default="4"/>
-    <configure  name="ANDI_NUM_VIRTUAL_ACT" default="2"/>
-    <configure  name="ANDI_NUM_ACT_TOT" default="6"/>
+    <configure  name="ANDI_OUTPUTS" default="6"/>
     <define     name="ANDI_OUTPUTS" value="$(ANDI_OUTPUTS)"/>
-    <define     name="ANDI_NUM_ACT" value="$(ANDI_NUM_ACT)"/>
-    <define     name="ANDI_NUM_VIRTUAL_ACT" value="$(ANDI_NUM_VIRTUAL_ACT)"/>
-    <define     name="ANDI_NUM_ACT_TOT" value="$(ANDI_NUM_ACT_TOT)"/>
   </makefile> 
 </module>

sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c

@@ -95,28 +95,6 @@
 #include <stdio.h>
 
 // Number of real actuators (e.g. motors, servos)
-#ifndef ANDI_NUM_ACT
-#error "You must specify the number of real actuators"
-#define ANDI_NUM_ACT 4
-#endif
-
-// Number of virtual actuators (e.g. Phi, Theta). For now 2 and only 2 are supported but in the future this can be further developed. 
-#if ANDI_NUM_VIRTUAL_ACT < 2
-#error "You must specify the number of virtual actuators to be at least 2"
-#define ANDI_NUM_VIRTUAL_ACT 2
-#endif
-
-// If the total number of actuators is not defined or wrongly defined correct it
-#if ANDI_NUM_ACT_TOT != (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT)
-#error "The number of actuators is not equal to the sum of real and virtual actuators"
-#define ANDI_NUM_ACT_TOT (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT)
-#endif
-
-#ifndef ANDI_OUTPUTS
-#error "You must specify the number of controlled axis (outputs)"
-#define ANDI_OUTPUTS 6
-#endif
-
 #ifndef ONELOOP_ANDI_NUM_THRUSTERS
 float num_thrusters_oneloop = 4.0; // Number of motors used for thrust
 #else

sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h

@@ -29,7 +29,28 @@
 #include "firmwares/rotorcraft/stabilization.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_common_int.h"
 #include "firmwares/rotorcraft/stabilization/stabilization_attitude_ref_quat_int.h"
+#include "generated/airframe.h"
 
+#ifndef ANDI_NUM_ACT
+#define ANDI_NUM_ACT COMMANDS_NB_REAL
+#endif
+
+#ifndef ANDI_NUM_VIRTUAL_ACT
+#define ANDI_NUM_VIRTUAL_ACT COMMANDS_NB_VIRTUAL
+#endif
+
+// Number of virtual actuators (e.g. Phi, Theta). For now 2 and only 2 are supported but in the future this can be further developed. 
+#if ANDI_NUM_VIRTUAL_ACT < 2
+#error "You must specify the number of virtual actuators to be at least 2"
+#define ANDI_NUM_VIRTUAL_ACT 2
+#endif
+
+#define ANDI_NUM_ACT_TOT (ANDI_NUM_ACT + ANDI_NUM_VIRTUAL_ACT)
+
+#ifndef ANDI_OUTPUTS
+#error "You must specify the number of controlled axis (outputs)"
+#define ANDI_OUTPUTS 6
+#endif
 #define ANDI_G_SCALING 1000.0f
 
 extern float act_state_filt_vect_1l[ANDI_NUM_ACT];

sw/simulator/nps/nps_autopilot.h

@@ -38,9 +38,11 @@ extern "C" {
 #ifndef NPS_COMMANDS_NB
 #if defined MOTOR_MIXING_NB_MOTOR
 #define NPS_COMMANDS_NB MOTOR_MIXING_NB_MOTOR
+#elif defined NPS_USE_COMMANDS
+#define NPS_COMMANDS_NB COMMANDS_NB
 #else
 #define NPS_COMMANDS_NB ACTUATORS_NB  // uses actuators_pprz[ACTUATORS_NB]
-#endif /* #if defined MOTOR_MIXING_NB_MOTOR */
+#endif /* #if defined MOTOR_MIXING_NB_MOTOR or NPS_USE_COMMANDS */
 #endif /* #ifndef NPS_COMMANDS_NB */
 
 struct NpsAutopilot {

sw/simulator/nps/nps_fdm_jsbsim.cpp

@@ -80,6 +80,19 @@
 #define JSBSIM_PATH(_x) _x
 #endif
 
+/* Actuator naming*/
+#ifdef NPS_USE_COMMANDS
+#define NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES TRUE
+#endif
+
+#ifdef NPS_ACTUATOR_NAMES
+#define NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES TRUE
+#endif
+
+#ifndef NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES
+#define NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES FALSE
+#endif
+
 /** Name of the JSBSim model.
  *  Defaults to the AIRFRAME_NAME
  */
@@ -304,19 +317,21 @@ void nps_fdm_set_temperature(double temp, double h)
  */
 static void feed_jsbsim(double *commands, int commands_nb __attribute__((unused)))
 {
-#ifdef NPS_ACTUATOR_NAMES
+#if NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES
   char buf[64];
+#if defined(NPS_USE_COMMANDS)
+  const char *names[] = COMMAND_NAMES; 
+#elif defined(NPS_ACTUATOR_NAMES)
   const char *names[] = NPS_ACTUATOR_NAMES;
+#endif 
   string property;
-
   int i;
   for (i = 0; i < commands_nb; i++) {
     sprintf(buf, "fcs/%s", names[i]);
     property = string(buf);
     FDMExec->GetPropertyManager()->GetNode(property)->SetDouble("", commands[i]);
   }
-#else /* use COMMAND names */
-
+#else /*Use manually defined commands*/
   // get FGFCS instance
   FGFCS *FCS = FDMExec->GetFCS();
 
@@ -355,7 +370,7 @@ static void feed_jsbsim(double *commands, int commands_nb __attribute__((unused)
   FCS->SetDfCmd(commands[COMMAND_FLAP]);
 #endif /* COMMAND_FLAP */
 
-#endif /* NPS_ACTUATOR_NAMES */
+#endif /* NPS_AUTOMATIC_JSBSIM_ACTUATOR_NAMES */
 }
 
 /**

sw/tools/generators/gen_airframe.ml

@@ -31,7 +31,7 @@ open Printf
 open Xml2h
 
 type channel = { min : float; max : float; neutral : float }
-type control = { failsafe_value : int; foo : int }
+type control = { failsafe_value : int; foo : int; group : string}
 
 let fos = float_of_string
 let sof = fun x -> if mod_float x 1. = 0. then Printf.sprintf "%.0f" x else string_of_float x
@@ -337,9 +337,22 @@ 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
-  define_out out command_name (string_of_int no);
   let failsafe_value = int_of_string (ExtXml.attrib command "failsafe_value") in
-  { failsafe_value = failsafe_value; foo = 0}
+  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 }  
+
+let count_commands_by_type commands_params =
+  Array.fold_left (fun acc cmd ->
+    let subtype = cmd.group in
+    let count = try List.assoc subtype acc with Not_found -> 0 in
+    (subtype, count + 1) :: (List.remove_assoc subtype acc)
+  ) [] commands_params
+
+let generate_command_names = fun out commands ->
+  let command_names = Array.map (fun axis -> "\"" ^ (ExtXml.attrib axis "name") ^ "\"") commands in
+  let command_names = String.concat ", " (Array.to_list command_names) in
+  fprintf out "#define COMMAND_NAMES { %s }\n\n" command_names
 
 let parse_heli_curves = fun out heli_curves ->
   let a = fun s -> ExtXml.attrib heli_curves s in
@@ -387,7 +400,12 @@ let rec parse_section = fun out ac_id s ->
     | "commands" ->
       let commands = Array.of_list (Xml.children s) in
       let commands_params = Array.mapi (fun i c -> parse_command out c i) commands in
+      let commands_counts = count_commands_by_type (commands_params) in
+      List.iter (fun (subtype, count) ->
+        define_out out (sprintf "COMMANDS_NB_%s" subtype) (string_of_int count)
+      ) commands_counts;      
       define_out out "COMMANDS_NB" (string_of_int (Array.length commands));
+      generate_command_names out commands;
       define_out out "COMMANDS_FAILSAFE" (sprint_float_array (List.map (fun x -> string_of_int x.failsafe_value) (Array.to_list commands_params)));
       fprintf out "\n\n"
     | "rc_commands" ->