[nps] fix number of actuator commands used

This makes it finally possible to have e.g. a hexarotor:
Six "commands" (one per motor) from motor_mixing sent to the FDM.

sw/simulator/nps/nps_autopilot.h

@@ -5,8 +5,22 @@
 
 #include "nps_radio_control.h"
 
+/**
+ * Number of commands sent to the FDM of NPS.
+ * If MOTOR_MIXING_NB_MOTOR is defined (usually rotorcraft firmware)
+ * we have that many commands (one per motor),
+ * otherwise we default to the number of high level commands (COMMANDS_NB).
+ */
+#ifndef NPS_COMMANDS_NB
+#if defined MOTOR_MIXING_NB_MOTOR
+#define NPS_COMMANDS_NB MOTOR_MIXING_NB_MOTOR
+#else
+#define NPS_COMMANDS_NB COMMANDS_NB
+#endif
+#endif
+
 struct NpsAutopilot {
-  double commands[COMMANDS_NB];
+  double commands[NPS_COMMANDS_NB];
 };
 
 extern struct NpsAutopilot autopilot;

sw/simulator/nps/nps_autopilot_fixedwing.c

@@ -134,7 +134,7 @@ void nps_autopilot_run_step(double time) {
   Ap(handle_periodic_tasks);
 
   /* scale final motor commands to 0-1 for feeding the fdm */
-  for (uint8_t i=0; i < COMMANDS_NB; i++)
+  for (uint8_t i=0; i < NPS_COMMANDS_NB; i++)
     autopilot.commands[i] = (double)commands[i]/MAX_PPRZ;
 
 }

sw/simulator/nps/nps_autopilot_rotorcraft.c

@@ -52,6 +52,9 @@ bool_t nps_bypass_ins;
 #define NPS_BYPASS_INS FALSE
 #endif
 
+#if NPS_COMMANDS_NB != MOTOR_MIXING_NB_MOTOR
+#error "NPS_COMMANDS_NB does not match MOTOR_MIXING_NB_MOTOR!"
+#endif
 
 void nps_autopilot_init(enum NpsRadioControlType type_rc, int num_rc_script, char* rc_dev) {
 
@@ -114,8 +117,7 @@ void nps_autopilot_run_step(double time) {
   handle_periodic_tasks();
 
   /* scale final motor commands to 0-1 for feeding the fdm */
-  /* FIXME: autopilot.commands is of length NB_COMMANDS instead of number of motors */
+  for (uint8_t i=0; i < NPS_COMMANDS_NB; i++)
-  for (uint8_t i=0; i<MOTOR_MIXING_NB_MOTOR; i++)
     autopilot.commands[i] = (double)motor_mixing.commands[i]/MAX_PPRZ;
 
 }

sw/simulator/nps/nps_fdm.h

@@ -93,7 +93,7 @@ struct NpsFdm {
 extern struct NpsFdm fdm;
 
 extern void nps_fdm_init(double dt);
-extern void nps_fdm_run_step(double* commands);
+extern void nps_fdm_run_step(double* commands, int commands_nb);
 extern void nps_fdm_set_wind(double speed, double dir, int turbulence_severity);
 
 #endif /* NPS_FDM */

sw/simulator/nps/nps_fdm_crrcsim.c

@@ -141,7 +141,7 @@ void nps_fdm_init(double dt) {
   send_servo_cmd(&crrcsim, zero);
 }
 
-void nps_fdm_run_step(double* commands) {
+void nps_fdm_run_step(double* commands, int commands_nb) {
   // read state
   if (get_msg(&crrcsim, crrcsim.data_buffer) <= 0) {
     return; // nothing on the socket

sw/simulator/nps/nps_fdm_jsbsim.c

@@ -71,7 +71,7 @@
 using namespace JSBSim;
 using namespace std;
 
-static void feed_jsbsim(double* commands);
+static void feed_jsbsim(double* commands, int commands_nb);
 static void fetch_state(void);
 static int check_for_nan(void);
 
@@ -124,9 +124,9 @@ void nps_fdm_init(double dt) {
 
 }
 
-void nps_fdm_run_step(double* commands) {
+void nps_fdm_run_step(double* commands, int commands_nb) {
 
-  feed_jsbsim(commands);
+  feed_jsbsim(commands, commands_nb);
 
   /* To deal with ground interaction issues, we decrease the time
      step as the vehicle is close to the ground. This is done predictively
@@ -193,16 +193,17 @@ void nps_fdm_set_wind(double speed, double dir, int turbulence_severity) {
 /**
  * Feed JSBSim with the latest actuator commands.
  *
- * @param commands   Pointer to array of doubles holding actuator commands
+ * @param commands    Pointer to array of doubles holding actuator commands
+ * @param commands_nb Number of commands (length of array)
  */
-static void feed_jsbsim(double* commands) {
+static void feed_jsbsim(double* commands, int commands_nb) {
 
   char buf[64];
   const char* names[] = NPS_ACTUATOR_NAMES;
   string property;
 
   int i;
-  for (i=0; i<COMMANDS_NB; i++) {
+  for (i=0; i < commands_nb; i++) {
     sprintf(buf,"fcs/%s",names[i]);
     property = string(buf);
     FDMExec->GetPropertyManager()->SetDouble(property,commands[i]);

sw/simulator/nps/nps_main.c

@@ -158,7 +158,7 @@ static void nps_main_run_sim_step(void) {
 
   nps_autopilot_run_systime_step();
 
-  nps_fdm_run_step(autopilot.commands);
+  nps_fdm_run_step(autopilot.commands, NPS_COMMANDS_NB);
 
   nps_sensors_run_step(nps_main.sim_time);