diff --git a/conf/firmwares/lisa_passthrough.makefile b/conf/firmwares/lisa_passthrough.makefile
index 84d7c3d876..9bda00c80b 100644
--- a/conf/firmwares/lisa_passthrough.makefile
+++ b/conf/firmwares/lisa_passthrough.makefile
@@ -60,7 +60,7 @@ stm_passthrough.srcs += $(SRC_LISA)/lisa_overo_link.c           \
 stm_passthrough.srcs += math/pprz_trig_int.c
 stm_passthrough.srcs += lisa/plug_sys.c
 
-stm_passthrough.srcs += $(SRC_FIRMWARE)/commands.c
+stm_passthrough.srcs += subsystems/commands.c
 
 # Radio control
 #
diff --git a/conf/firmwares/lisa_test_progs.makefile b/conf/firmwares/lisa_test_progs.makefile
index c5d55b2e43..281bdc3187 100644
--- a/conf/firmwares/lisa_test_progs.makefile
+++ b/conf/firmwares/lisa_test_progs.makefile
@@ -508,7 +508,7 @@ test_actuators_mkk.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
 test_actuators_mkk.srcs   += $(COMMON_TELEMETRY_SRCS)
 
 test_actuators_mkk.srcs   += test/test_actuators.c
-test_actuators_mkk.srcs   += $(SRC_FIRMWARE)/commands.c
+test_actuators_mkk.srcs   += subsystems/commands.c
 test_actuators_mkk.srcs   += $(SRC_FIRMWARE)/actuators/actuators_mkk.c
 test_actuators_mkk.CFLAGS += -DACTUATORS_MKK_DEVICE=i2c1
 test_actuators_mkk.srcs   += $(SRC_FIRMWARE)/actuators/supervision.c
@@ -526,7 +526,7 @@ test_actuators_asctecv1.CFLAGS += $(COMMON_TELEMETRY_CFLAGS)
 test_actuators_asctecv1.srcs   += $(COMMON_TELEMETRY_SRCS)
 
 test_actuators_asctecv1.srcs   += test/test_actuators.c
-test_actuators_asctecv1.srcs   += $(SRC_FIRMWARE)/commands.c
+test_actuators_asctecv1.srcs   += subsystems/commands.c
 test_actuators_asctecv1.CFLAGS += -DACTUATORS_ASCTEC_DEVICE=i2c1
 test_actuators_asctecv1.srcs   += $(SRC_FIRMWARE)/actuators/actuators_asctec.c
 test_actuators_asctecv1.CFLAGS += -DUSE_I2C1
@@ -560,7 +560,7 @@ test_actuators_asctecv1.srcs   += mcu_periph/i2c.c $(SRC_ARCH)/mcu_periph/i2c_ar
 #test_manual.srcs   += $(COMMON_TELEMETRY_SRCS)
 #
 #test_manual.srcs   += test/test_manual.c
-#test_manual.srcs   += $(SRC_FIRMWARE)/commands.c
+#test_manual.srcs   += subsystems/commands.c
 ##test_manual.srcs   += subsystems/actuators/actuators_pwm.c
 #test_manual.srcs   += $(SRC_ARCH)/subsystems/actuators/actuators_pwm_arch.c
 #
diff --git a/conf/firmwares/rotorcraft.makefile b/conf/firmwares/rotorcraft.makefile
index c46f2c7715..a67e181dc8 100644
--- a/conf/firmwares/rotorcraft.makefile
+++ b/conf/firmwares/rotorcraft.makefile
@@ -104,7 +104,7 @@ ifeq ($(TARGET), ap)
   include $(CFG_SHARED)/i2c_select.makefile
 endif
 
-ap.srcs += $(SRC_FIRMWARE)/commands.c
+ap.srcs += subsystems/commands.c
 ap.srcs += subsystems/actuators.c
 
 #
diff --git a/conf/firmwares/subsystems/fixedwing/autopilot.makefile b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
index 6d8fc2507d..cb424266aa 100644
--- a/conf/firmwares/subsystems/fixedwing/autopilot.makefile
+++ b/conf/firmwares/subsystems/fixedwing/autopilot.makefile
@@ -152,7 +152,7 @@ endif
 fbw_CFLAGS		+= -DFBW
 fbw_srcs 		+= $(SRC_FIRMWARE)/main_fbw.c
 fbw_srcs 		+= subsystems/electrical.c
-fbw_srcs 		+= $(SRC_FIXEDWING)/commands.c
+fbw_srcs 		+= subsystems/commands.c
 fbw_srcs 		+= subsystems/actuators.c
 fbw_srcs		+= $(SRC_FIRMWARE)/fbw_downlink.c
 
diff --git a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
index 99663ac7c6..22b8ee11e6 100644
--- a/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
+++ b/conf/firmwares/subsystems/rotorcraft/fdm_jsbsim.makefile
@@ -83,7 +83,7 @@ nps.srcs += $(SRC_FIRMWARE)/telemetry.c \
             $(SRC_ARCH)/ivy_transport.c
 
 nps.srcs   += subsystems/actuators.c
-nps.srcs   += $(SRC_FIRMWARE)/commands.c
+nps.srcs   += subsystems/commands.c
 
 nps.srcs += $(SRC_FIRMWARE)/datalink.c
 
diff --git a/sw/airborne/arch/sim/jsbsim_hw.h b/sw/airborne/arch/sim/jsbsim_hw.h
index fa32784663..27406b89a6 100644
--- a/sw/airborne/arch/sim/jsbsim_hw.h
+++ b/sw/airborne/arch/sim/jsbsim_hw.h
@@ -40,7 +40,7 @@
 #include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
 #include "firmwares/fixedwing/guidance/guidance_v.h"
 #include "subsystems/sensors/infrared.h"
-#include "commands.h"
+#include "subsystems/commands.h"
 #include "firmwares/fixedwing/main_ap.h"
 #include "ap_downlink.h"
 #include "sim_uart.h"
diff --git a/sw/airborne/arch/sim/sim_ap.c b/sw/airborne/arch/sim/sim_ap.c
index 7eaffac836..4a9a2ec3c4 100644
--- a/sw/airborne/arch/sim/sim_ap.c
+++ b/sw/airborne/arch/sim/sim_ap.c
@@ -18,7 +18,7 @@
 #include "subsystems/nav.h"
 #include "firmwares/fixedwing/stabilization/stabilization_attitude.h"
 #include "firmwares/fixedwing/guidance/guidance_v.h"
-#include "commands.h"
+#include "subsystems/commands.h"
 #include "firmwares/fixedwing/main_ap.h"
 #include "ap_downlink.h"
 #include "sim_uart.h"
diff --git a/sw/airborne/firmwares/beth/main_stm32.c b/sw/airborne/firmwares/beth/main_stm32.c
index af702db5ee..a1e1b7c8c8 100644
--- a/sw/airborne/firmwares/beth/main_stm32.c
+++ b/sw/airborne/firmwares/beth/main_stm32.c
@@ -28,7 +28,7 @@
 #include "mcu_periph/can.h"
 #include "mcu_periph/sys_time.h"
 #include "subsystems/datalink/downlink.h"
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/commands.h"
 #include "firmwares/rotorcraft/actuators.h"
 //#include "booz/booz_radio_control.h"
 #include "subsystems/imu.h"
diff --git a/sw/airborne/firmwares/fixedwing/autopilot.h b/sw/airborne/firmwares/fixedwing/autopilot.h
index cbc0253d97..8c9cc9ab50 100644
--- a/sw/airborne/firmwares/fixedwing/autopilot.h
+++ b/sw/airborne/firmwares/fixedwing/autopilot.h
@@ -33,6 +33,7 @@
 #include <inttypes.h>
 #include "std.h"
 #include "mcu_periph/sys_time.h"
+#include "generated/airframe.h"
 
 #define THRESHOLD_MANUAL_PPRZ (MIN_PPRZ / 2)
 
@@ -57,6 +58,10 @@ extern bool_t kill_throttle;
 /** flight time in seconds. */
 extern uint16_t autopilot_flight_time;
 
+#define autopilot_ResetFlightTimeAndLaunch(_) { \
+  autopilot_flight_time = 0; launch = FALSE; \
+}
+
 
 // FIXME, move to control
 #define LATERAL_MODE_MANUAL    0
@@ -85,6 +90,22 @@ extern bool_t gps_lost;
   (_mode != new_mode ? _mode = new_mode, TRUE : FALSE); \
 })
 
+/** Commands trim for roll and pitch/
+ */
+#ifndef COMMAND_ROLL_TRIM
+#define COMMAND_ROLL_TRIM 0
+#endif
+
+#ifndef COMMAND_PITCH_TRIM
+#define COMMAND_PITCH_TRIM 0
+#endif
+
+extern pprz_t command_roll_trim = COMMAND_ROLL_TRIM;
+extern pprz_t command_pitch_trim = COMMAND_PITCH_TRIM;
+
+
+/** Power switch control.
+ */
 extern bool_t power_switch;
 
 #ifdef POWER_SWITCH_LED
@@ -96,9 +117,6 @@ extern bool_t power_switch;
 #define autopilot_SetPowerSwitch(_x) { power_switch = _x; }
 #endif // POWER_SWITCH_LED
 
-#define autopilot_ResetFlightTimeAndLaunch(_) { \
-  autopilot_flight_time = 0; launch = FALSE; \
-}
 
 /* CONTROL_RATE will be removed in the next release
  * please use CONTROL_FREQUENCY instead
diff --git a/sw/airborne/firmwares/fixedwing/fbw_downlink.h b/sw/airborne/firmwares/fixedwing/fbw_downlink.h
index 0bc8cd48ab..19947da37e 100644
--- a/sw/airborne/firmwares/fixedwing/fbw_downlink.h
+++ b/sw/airborne/firmwares/fixedwing/fbw_downlink.h
@@ -39,7 +39,7 @@
 #include "messages.h"
 #include "generated/periodic_telemetry.h"
 #include "generated/airframe.h"
-#include "commands.h"
+#include "subsystems/commands.h"
 #include "subsystems/actuators.h"
 
 #include "mcu_periph/uart.h"
diff --git a/sw/airborne/firmwares/fixedwing/main_fbw.c b/sw/airborne/firmwares/fixedwing/main_fbw.c
index 14c298e9c4..544ac2e36a 100644
--- a/sw/airborne/firmwares/fixedwing/main_fbw.c
+++ b/sw/airborne/firmwares/fixedwing/main_fbw.c
@@ -36,7 +36,7 @@
 #include "firmwares/fixedwing/main_fbw.h"
 #include "mcu.h"
 #include "mcu_periph/sys_time.h"
-#include "commands.h"
+#include "subsystems/commands.h"
 #include "subsystems/actuators.h"
 #include "subsystems/electrical.h"
 #include "subsystems/radio_control.h"
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c
index 478683ba41..fd8b4c7524 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.c
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.c
@@ -23,7 +23,7 @@
 
 #include "subsystems/radio_control.h"
 #include "subsystems/gps.h"
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/commands.h"
 #include "firmwares/rotorcraft/navigation.h"
 #include "firmwares/rotorcraft/guidance.h"
 #include "firmwares/rotorcraft/stabilization.h"
@@ -102,15 +102,14 @@ void autopilot_periodic(void) {
 #else
   if (autopilot_mode == AP_MODE_KILL) {
 #endif
-    SetCommands(commands_failsafe,
-		autopilot_in_flight, autopilot_motors_on);
+    SetCommands(commands_failsafe);
   }
   else {
     guidance_v_run( autopilot_in_flight );
     guidance_h_run( autopilot_in_flight );
-    SetCommands(stabilization_cmd,
-        autopilot_in_flight, autopilot_motors_on);
+    SetCommands(stabilization_cmd);
   }
+  RotorcraftCommandsTest(commands, autopilot_in_flight, autopilot_motors_on);
 
 }
 
diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h
index d3fbfb1df3..9ad782dea9 100644
--- a/sw/airborne/firmwares/rotorcraft/autopilot.h
+++ b/sw/airborne/firmwares/rotorcraft/autopilot.h
@@ -65,8 +65,8 @@ extern bool_t autopilot_detect_ground_once;
 
 extern uint16_t autopilot_flight_time;
 
-
-
+/** Default RC mode.
+ */
 #ifndef MODE_MANUAL
 #define MODE_MANUAL AP_MODE_RATE_DIRECT
 #endif
@@ -109,6 +109,23 @@ extern uint16_t autopilot_flight_time;
   }
 #endif
 
+/** Thrust and Yaw commands limitation.
+ *  Limit thrust and/or yaw depending of the in_flight
+ *  and motors_on flag status
+ */
+#ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
+#define RotorcraftCommandsTest(_cmd, _in_flight,  _motor_on) {  \
+  if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; }                 \
+  if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; }               \
+}
+#else
+#define RotorcraftCommandsTest(_cmd, _in_flight,  _motor_on) {  \
+  if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; }               \
+}
+#endif
+
+/** Ground detection based on accelerometers.
+ */
 #ifndef THRESHOLD_GROUND_DETECT
 #define THRESHOLD_GROUND_DETECT ACCEL_BFP_OF_REAL(15.)
 #endif
diff --git a/sw/airborne/firmwares/rotorcraft/commands.c b/sw/airborne/firmwares/rotorcraft/commands.c
deleted file mode 100644
index a07e5fe486..0000000000
--- a/sw/airborne/firmwares/rotorcraft/commands.c
+++ /dev/null
@@ -1,26 +0,0 @@
-/*  $Id$
- *
- * (c) 2009 Antoine Drouin <poinix@gmail.com>
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- */
-
-#include "firmwares/rotorcraft/commands.h"
-
-int32_t commands[COMMANDS_NB];
-const int32_t commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;
diff --git a/sw/airborne/firmwares/rotorcraft/commands.h b/sw/airborne/firmwares/rotorcraft/commands.h
deleted file mode 100644
index fa1cb57842..0000000000
--- a/sw/airborne/firmwares/rotorcraft/commands.h
+++ /dev/null
@@ -1,49 +0,0 @@
-/*  $Id$
- *
- * (c) 2009 Antoine Drouin <poinix@gmail.com>
- *
- * This file is part of paparazzi.
- *
- * paparazzi is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License as published by
- * the Free Software Foundation; either version 2, or (at your option)
- * any later version.
- *
- * paparazzi is distributed in the hope that it will be useful,
- * but WITHOUT ANY WARRANTY; without even the implied warranty of
- * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
- * GNU General Public License for more details.
- *
- * You should have received a copy of the GNU General Public License
- * along with paparazzi; see the file COPYING.  If not, write to
- * the Free Software Foundation, 59 Temple Place - Suite 330,
- * Boston, MA 02111-1307, USA.
- *
- */
-
-#ifndef COMMANDS_H
-#define COMMANDS_H
-
-#include "paparazzi.h"
-#include "generated/airframe.h"
-
-extern int32_t commands[COMMANDS_NB];
-extern const int32_t commands_failsafe[COMMANDS_NB];
-
-#ifndef ROTORCRAFT_COMMANDS_YAW_ALWAYS_ENABLED
-#define SetCommands(_in_cmd, _in_flight, _motors_on) {			\
-    commands[COMMAND_PITCH]  = _in_cmd[COMMAND_PITCH];		\
-    commands[COMMAND_ROLL]   = _in_cmd[COMMAND_ROLL];		\
-    commands[COMMAND_YAW]    = (_in_flight) ? _in_cmd[COMMAND_YAW] : 0; \
-    commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \
-  }
-#else
-#define SetCommands(_in_cmd, _in_flight, _motors_on) {			\
-    commands[COMMAND_PITCH]  = _in_cmd[COMMAND_PITCH];		\
-    commands[COMMAND_ROLL]   = _in_cmd[COMMAND_ROLL];		\
-    commands[COMMAND_YAW]    = _in_cmd[COMMAND_YAW]; \
-    commands[COMMAND_THRUST] = (_motors_on) ? _in_cmd[COMMAND_THRUST] : 0; \
-  }
-#endif
-
-#endif /* COMMANDS_H */
diff --git a/sw/airborne/firmwares/rotorcraft/main.c b/sw/airborne/firmwares/rotorcraft/main.c
index e927c539a8..3218dec115 100644
--- a/sw/airborne/firmwares/rotorcraft/main.c
+++ b/sw/airborne/firmwares/rotorcraft/main.c
@@ -35,7 +35,7 @@
 #include "subsystems/settings.h"
 #include "subsystems/datalink/xbee.h"
 
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/commands.h"
 #include "subsystems/actuators.h"
 #if USE_MOTOR_MIXING
 #include "subsystems/actuators/motor_mixing.h"
diff --git a/sw/airborne/link_mcu_usart.c b/sw/airborne/link_mcu_usart.c
index ec93d06f40..ebbf2201ae 100644
--- a/sw/airborne/link_mcu_usart.c
+++ b/sw/airborne/link_mcu_usart.c
@@ -24,7 +24,7 @@
 #include "mcu_periph/uart.h"
 #include "led.h"
 
-#include "commands.h"
+#include "subsystems/commands.h"
 
 //////////////////////////////////////////////////////////////////////////////////////////////
 // LINK
diff --git a/sw/airborne/lisa/lisa_stm_passthrough_main.c b/sw/airborne/lisa/lisa_stm_passthrough_main.c
index aeda89aa54..c2b61689b9 100644
--- a/sw/airborne/lisa/lisa_stm_passthrough_main.c
+++ b/sw/airborne/lisa/lisa_stm_passthrough_main.c
@@ -26,7 +26,7 @@
 #include "mcu_periph/uart.h"
 #include "mcu_periph/sys_time.h"
 #include "subsystems/datalink/downlink.h"
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/commands.h"
 #include "subsystems/actuators.h"
 #include "subsystems/actuators/actuators_pwm.h"
 #include "subsystems/imu.h"
diff --git a/sw/airborne/lisa/test/lisa_test_actuators_mkk.c b/sw/airborne/lisa/test/lisa_test_actuators_mkk.c
index cd86efe09b..25ab7e7e6f 100644
--- a/sw/airborne/lisa/test/lisa_test_actuators_mkk.c
+++ b/sw/airborne/lisa/test/lisa_test_actuators_mkk.c
@@ -24,7 +24,7 @@
 
 #include "mcu.h"
 #include "mcu_periph/sys_time.h"
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/commands.h"
 #include "subsystems/actuators.h"
 #include "subsystems/datalink/downlink.h"
 #include "led.h"
diff --git a/sw/airborne/commands.c b/sw/airborne/subsystems/commands.c
similarity index 79%
rename from sw/airborne/commands.c
rename to sw/airborne/subsystems/commands.c
index 3dd61cfdd3..b03927465c 100644
--- a/sw/airborne/commands.c
+++ b/sw/airborne/subsystems/commands.c
@@ -1,5 +1,4 @@
-/*  $Id$
- *
+/*
  * (c) 2006 Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
@@ -26,18 +25,7 @@
  *
  */
 
-#include "commands.h"
-
-#ifndef COMMAND_ROLL_TRIM
-#define COMMAND_ROLL_TRIM 0
-#endif
-
-#ifndef COMMAND_PITCH_TRIM
-#define COMMAND_PITCH_TRIM 0
-#endif
-
-pprz_t command_roll_trim = COMMAND_ROLL_TRIM;
-pprz_t command_pitch_trim = COMMAND_PITCH_TRIM;
+#include "subsystems/commands.h"
 
 pprz_t commands[COMMANDS_NB];
 const pprz_t commands_failsafe[COMMANDS_NB] = COMMANDS_FAILSAFE;
diff --git a/sw/airborne/commands.h b/sw/airborne/subsystems/commands.h
similarity index 81%
rename from sw/airborne/commands.h
rename to sw/airborne/subsystems/commands.h
index 4716959233..762afac8c3 100644
--- a/sw/airborne/commands.h
+++ b/sw/airborne/subsystems/commands.h
@@ -1,5 +1,4 @@
-/*  $Id$
- *
+/*
  * (c) 2006 Pascal Brisset, Antoine Drouin
  *
  * This file is part of paparazzi.
@@ -32,14 +31,12 @@
 #include "paparazzi.h"
 #include "generated/airframe.h"
 
-extern pprz_t command_roll_trim;
-extern pprz_t command_pitch_trim;
-
+/** Storage of intermediate command values.
+ *  These values come from the RC (MANUAL mode), from the autopilot (AUTO mode) or from control loops.
+ *  They are asyncronisly used to set the servos
+ */
 extern pprz_t commands[COMMANDS_NB];
 extern const pprz_t commands_failsafe[COMMANDS_NB];
-/** Storage of intermediate command values: these values come from
-the RC (MANUAL mode), from the autopilot (AUTO mode) or from control loops.
-They are asyncronisly used to set the servos */
 
 #define SetCommands(t) { \
   int i; \
diff --git a/sw/airborne/subsystems/electrical.c b/sw/airborne/subsystems/electrical.c
index 2d9c1d192c..de1a0e8b8d 100644
--- a/sw/airborne/subsystems/electrical.c
+++ b/sw/airborne/subsystems/electrical.c
@@ -1,7 +1,7 @@
 #include "subsystems/electrical.h"
 
 #include "mcu_periph/adc.h"
-#include "commands.h"
+#include "subsystems/commands.h"
 
 #include "generated/airframe.h"
 #include BOARD_CONFIG
diff --git a/sw/airborne/test/test_actuators.c b/sw/airborne/test/test_actuators.c
index dc2d4eda10..d6e6c66a71 100644
--- a/sw/airborne/test/test_actuators.c
+++ b/sw/airborne/test/test_actuators.c
@@ -27,7 +27,7 @@
 #include "led.h"
 
 #include "mcu_periph/i2c.h"
-#include "firmwares/rotorcraft/commands.h"
+#include "subsystems/commands.h"
 #include "subsystems/actuators.h"
 
 static inline void main_init( void );