[rotorcraft] RC commands set when not in nav

Rotorcraft commands are set with a specific macro

sw/airborne/firmwares/rotorcraft/autopilot.c

@@ -114,9 +114,8 @@ void autopilot_periodic(void) {
   else {
     guidance_v_run( autopilot_in_flight );
     guidance_h_run( autopilot_in_flight );
-    SetCommands(stabilization_cmd);
+    SetRotorcraftCommands(stabilization_cmd, autopilot_in_flight, autopilot_motors_on);
   }
-  RotorcraftCommandsTest(commands, autopilot_in_flight, autopilot_motors_on);
 
 }
 
@@ -268,9 +267,9 @@ void autopilot_on_rc_frame(void) {
     SetAutoCommandsFromRC(commands, radio_control.values);
 #endif
 
-    /* if in "MANUAL" set commands from the rc */
+    /* if not in NAV_MODE set commands from the rc */
 #ifdef SetCommandsFromRC
-    if (autopilot_mode == MODE_MANUAL) {
+    if (autopilot_mode != AP_MODE_NAV) {
       SetCommandsFromRC(commands, radio_control.values);
     }
 #endif

sw/airborne/firmwares/rotorcraft/autopilot.h

@@ -116,18 +116,26 @@ extern uint16_t autopilot_flight_time;
   }
 #endif
 
-/** Thrust and Yaw commands limitation.
+/** Set Rotorcraft commands.
  *  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; }               \
+#define SetRotorcraftCommands(_cmd, _in_flight,  _motor_on) { \
+  if (!(_in_flight)) { _cmd[COMMAND_YAW] = 0; }               \
+  if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; }             \
+  commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL];                \
+  commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH];              \
+  commands[COMMAND_YAW] = _cmd[COMMAND_YAW];                  \
+  commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST];            \
 }
 #else
-#define RotorcraftCommandsTest(_cmd, _in_flight,  _motor_on) {  \
-  if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; }               \
+#define SetRotorcraftCommands(_cmd, _in_flight,  _motor_on) { \
+  if (!(_motor_on)) { _cmd[COMMAND_THRUST] = 0; }             \
+  commands[COMMAND_ROLL] = _cmd[COMMAND_ROLL];                \
+  commands[COMMAND_PITCH] = _cmd[COMMAND_PITCH];              \
+  commands[COMMAND_YAW] = _cmd[COMMAND_YAW];                  \
+  commands[COMMAND_THRUST] = _cmd[COMMAND_THRUST];            \
 }
 #endif