From 73af245e1e19b14e07dcab7b3554db6ace7a0ad6 Mon Sep 17 00:00:00 2001 From: Felix Ruess Date: Mon, 9 Mar 2015 15:30:54 +0100 Subject: [PATCH] [nps] fixedwings: no need for NPS_ACTUATOR_NAMES If NPS_ACTUATOR_NAMES is not specified, use the standard throttle,roll,pitch and if used in airframe yaw commands for throttle, aileron, elevator and rudder in JSBSim model. --- conf/airframes/ENAC/fixed-wing/jp.xml | 4 +--- conf/airframes/examples/microjet_lisa_m.xml | 4 ---- sw/simulator/nps/nps_autopilot.h | 5 +++++ sw/simulator/nps/nps_autopilot_fixedwing.c | 24 +++++++++++++++++++-- sw/simulator/nps/nps_fdm_jsbsim.cpp | 13 +++++++++-- 5 files changed, 39 insertions(+), 11 deletions(-) diff --git a/conf/airframes/ENAC/fixed-wing/jp.xml b/conf/airframes/ENAC/fixed-wing/jp.xml index 888a767be8..f51b75e2eb 100644 --- a/conf/airframes/ENAC/fixed-wing/jp.xml +++ b/conf/airframes/ENAC/fixed-wing/jp.xml @@ -274,9 +274,7 @@
- - - +
diff --git a/conf/airframes/examples/microjet_lisa_m.xml b/conf/airframes/examples/microjet_lisa_m.xml index 422e469215..994512866d 100644 --- a/conf/airframes/examples/microjet_lisa_m.xml +++ b/conf/airframes/examples/microjet_lisa_m.xml @@ -204,12 +204,8 @@
- - - -
diff --git a/sw/simulator/nps/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h index 7aec46b786..ce18596952 100644 --- a/sw/simulator/nps/nps_autopilot.h +++ b/sw/simulator/nps/nps_autopilot.h @@ -15,7 +15,12 @@ #if defined MOTOR_MIXING_NB_MOTOR #define NPS_COMMANDS_NB MOTOR_MIXING_NB_MOTOR #else +#ifdef NPS_ACTUATOR_NAMES #define NPS_COMMANDS_NB COMMANDS_NB +#else +/* not using explicitly set NPS_ACTUATOR_NAMES -> throttle,roll,pitch,yaw commands */ +#define NPS_COMMANDS_NB 4 +#endif #endif #endif diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c index c995660d90..0e1bc09855 100644 --- a/sw/simulator/nps/nps_autopilot_fixedwing.c +++ b/sw/simulator/nps/nps_autopilot_fixedwing.c @@ -64,8 +64,6 @@ bool_t nps_bypass_ins; #define NPS_BYPASS_INS FALSE #endif -PRINT_CONFIG_VAR(NPS_COMMANDS_NB) - #if !defined (FBW) || !defined (AP) #error NPS does not currently support dual processor simulation for FBW and AP on fixedwing! @@ -139,10 +137,32 @@ void nps_autopilot_run_step(double time) { Ap(handle_periodic_tasks); /* scale final motor commands to 0-1 for feeding the fdm */ +#ifdef NPS_ACTUATOR_NAMES +PRINT_CONFIG_MSG("actuators for JSBSim explicitly set.") +PRINT_CONFIG_VAR(NPS_COMMANDS_NB) + //PRINT_CONFIG_VAR(NPS_ACTUATOR_NAMES) + for (uint8_t i=0; i < NPS_COMMANDS_NB; i++) autopilot.commands[i] = (double)commands[i]/MAX_PPRZ; // hack: invert pitch to fit most JSBSim models autopilot.commands[COMMAND_PITCH] = -(double)commands[COMMAND_PITCH]/MAX_PPRZ; +#else +PRINT_CONFIG_MSG("Using throttle, roll, pitch, yaw commands instead of explicit actuators.") +PRINT_CONFIG_VAR(COMMAND_THROTTLE) +PRINT_CONFIG_VAR(COMMAND_ROLL) +PRINT_CONFIG_VAR(COMMAND_PITCH) + + autopilot.commands[COMMAND_THROTTLE] = (double)commands[COMMAND_THROTTLE]/MAX_PPRZ; + autopilot.commands[COMMAND_ROLL] = (double)commands[COMMAND_ROLL]/MAX_PPRZ; + // hack: invert pitch to fit most JSBSim models + autopilot.commands[COMMAND_PITCH] = -(double)commands[COMMAND_PITCH]/MAX_PPRZ; +#ifdef COMMAND_YAW +PRINT_CONFIG_VAR(COMMAND_YAW) + autopilot.commands[COMMAND_YAW] = (double)commands[COMMAND_YAW]/MAX_PPRZ; +#else + autopilot.commands[3] = 0.; +#endif +#endif // do the launch when clicking launch in GCS autopilot.launch = launch && !kill_throttle; diff --git a/sw/simulator/nps/nps_fdm_jsbsim.cpp b/sw/simulator/nps/nps_fdm_jsbsim.cpp index fbdf9d6fb0..9f51965658 100644 --- a/sw/simulator/nps/nps_fdm_jsbsim.cpp +++ b/sw/simulator/nps/nps_fdm_jsbsim.cpp @@ -81,6 +81,7 @@ using namespace JSBSim; using namespace std; static void feed_jsbsim(double* commands, int commands_nb); +static void feed_jsbsim(double throttle, double aileron, double elevator, double rudder); static void fetch_state(void); static int check_for_nan(void); @@ -222,7 +223,7 @@ void nps_fdm_set_wind(double speed, double dir, int turbulence_severity) { * @param commands_nb Number of commands (length of array) */ static void feed_jsbsim(double* commands, int commands_nb) { - +#ifdef NPS_ACTUATOR_NAMES char buf[64]; const char* names[] = NPS_ACTUATOR_NAMES; string property; @@ -233,9 +234,17 @@ static void feed_jsbsim(double* commands, int commands_nb) { property = string(buf); FDMExec->GetPropertyManager()->GetNode(property)->SetDouble("", commands[i]); } +#else + if (commands_nb != 4) { + cerr << "commands_nb must be 4!" << endl; + exit(-1); + } + /* call version that directly feeds throttle, aileron, elevator, rudder */ + feed_jsbsim(commands[COMMAND_THROTTLE], commands[COMMAND_ROLL], commands[COMMAND_PITCH], commands[3]); +#endif } -void feed_jsbsim(double throttle, double aileron, double elevator, double rudder) +static void feed_jsbsim(double throttle, double aileron, double elevator, double rudder) { FGFCS* FCS = FDMExec->GetFCS(); FCS->SetDaCmd(aileron);