From 1cfdb7ace69d796b6862e76470bde82f12bf86fe Mon Sep 17 00:00:00 2001 From: Gautier Hattenberger Date: Mon, 7 Jan 2013 22:28:46 +0100 Subject: [PATCH] [rotorcraft] RC commands set when not in nav Rotorcraft commands are set with a specific macro --- sw/airborne/firmwares/rotorcraft/autopilot.c | 7 +++---- sw/airborne/firmwares/rotorcraft/autopilot.h | 20 ++++++++++++++------ 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.c b/sw/airborne/firmwares/rotorcraft/autopilot.c index 234b2013189..29479a1cb81 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.c +++ b/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 diff --git a/sw/airborne/firmwares/rotorcraft/autopilot.h b/sw/airborne/firmwares/rotorcraft/autopilot.h index 5323c9188b5..74431ac84e2 100644 --- a/sw/airborne/firmwares/rotorcraft/autopilot.h +++ b/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