diff --git a/sw/simulator/nps/nps_autopilot.h b/sw/simulator/nps/nps_autopilot.h
index 04d8304d0b..12b3052c4a 100644
--- a/sw/simulator/nps/nps_autopilot.h
+++ b/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;
diff --git a/sw/simulator/nps/nps_autopilot_fixedwing.c b/sw/simulator/nps/nps_autopilot_fixedwing.c
index 409a0c3a43..8f1952e0fd 100644
--- a/sw/simulator/nps/nps_autopilot_fixedwing.c
+++ b/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;
 
 }
diff --git a/sw/simulator/nps/nps_autopilot_rotorcraft.c b/sw/simulator/nps/nps_autopilot_rotorcraft.c
index 681d5bf5d8..ce0b529012 100644
--- a/sw/simulator/nps/nps_autopilot_rotorcraft.c
+++ b/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<MOTOR_MIXING_NB_MOTOR; i++)
+  for (uint8_t i=0; i < NPS_COMMANDS_NB; i++)
     autopilot.commands[i] = (double)motor_mixing.commands[i]/MAX_PPRZ;
 
 }
diff --git a/sw/simulator/nps/nps_fdm.h b/sw/simulator/nps/nps_fdm.h
index 99c0ec8691..99f79c7fdb 100644
--- a/sw/simulator/nps/nps_fdm.h
+++ b/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 */
diff --git a/sw/simulator/nps/nps_fdm_crrcsim.c b/sw/simulator/nps/nps_fdm_crrcsim.c
index d2223b8de0..5a39ecab4c 100644
--- a/sw/simulator/nps/nps_fdm_crrcsim.c
+++ b/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
diff --git a/sw/simulator/nps/nps_fdm_jsbsim.c b/sw/simulator/nps/nps_fdm_jsbsim.c
index 30a438d7e2..7102706eab 100644
--- a/sw/simulator/nps/nps_fdm_jsbsim.c
+++ b/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]);
diff --git a/sw/simulator/nps/nps_main.c b/sw/simulator/nps/nps_main.c
index 6686beb324..fd1475b97c 100644
--- a/sw/simulator/nps/nps_main.c
+++ b/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);