From 50694ba217528350264004720dd4f6ad964be390 Mon Sep 17 00:00:00 2001 From: Tomaso Maria Luigi De Ponti <48210579+tmldeponti@users.noreply.github.com> Date: Tue, 16 Apr 2024 23:06:23 +0200 Subject: [PATCH] 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 * Added logic to remove redundant code --------- Co-authored-by: Gautier Hattenberger --- conf/airframes/airframe.dtd | 3 ++- conf/modules/oneloop_andi.xml | 8 +----- .../rotorcraft/oneloop/oneloop_andi.c | 22 ---------------- .../rotorcraft/oneloop/oneloop_andi.h | 21 ++++++++++++++++ sw/simulator/nps/nps_autopilot.h | 4 ++- sw/simulator/nps/nps_fdm_jsbsim.cpp | 25 +++++++++++++++---- sw/tools/generators/gen_airframe.ml | 24 +++++++++++++++--- 7 files changed, 68 insertions(+), 39 deletions(-) diff --git a/conf/airframes/airframe.dtd b/conf/airframes/airframe.dtd index 3ce3c529e52..c2f9db70d4c 100644 --- a/conf/airframes/airframe.dtd +++ b/conf/airframes/airframe.dtd @@ -74,7 +74,8 @@ collective CDATA #REQUIRED> +failsafe_value CDATA #REQUIRED +group CDATA #IMPLIED> - - - - + - - - diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c index 8ed77e304f6..577108da3f8 100644 --- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c +++ b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.c @@ -95,28 +95,6 @@ #include // 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 diff --git a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h b/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h index 430be73ed7a..56b246cb7dd 100644 --- a/sw/airborne/firmwares/rotorcraft/oneloop/oneloop_andi.h +++ b/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]; diff --git a/sw/simulator/nps/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h index c227e51845b..801ac7ff66c 100644 --- a/sw/simulator/nps/nps_autopilot.h +++ b/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 { diff --git a/sw/simulator/nps/nps_fdm_jsbsim.cpp b/sw/simulator/nps/nps_fdm_jsbsim.cpp index 9c7ca40a737..0f9b00c82a8 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.cpp +++ b/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 */ } /** diff --git a/sw/tools/generators/gen_airframe.ml b/sw/tools/generators/gen_airframe.ml index 6289a79424b..9af1995dc08 100644 --- a/sw/tools/generators/gen_airframe.ml +++ b/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" ->