From 00bd1bf8ea12f1465d704baafbea51ce9012e647 Mon Sep 17 00:00:00 2001 From: Randy Mackay Date: Tue, 21 May 2013 10:58:34 +0900 Subject: [PATCH] Copter: RESETTOARMEDYAW order and comments We should keep the order in the case statement the same as the #define order. --- ArduCopter/ArduCopter.pde | 23 +++++++++++++---------- ArduCopter/control_modes.pde | 14 +++++++------- ArduCopter/defines.h | 2 +- 3 files changed, 21 insertions(+), 18 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 7b126869e3..a294c54581 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1376,8 +1376,6 @@ bool set_yaw_mode(uint8_t new_yaw_mode) } switch( new_yaw_mode ) { - case YAW_RESETTOARMEDYAW: - nav_yaw = ahrs.yaw_sensor; // store current yaw so we can start rotating back to correct one case YAW_HOLD: case YAW_ACRO: yaw_initialised = true; @@ -1411,14 +1409,18 @@ bool set_yaw_mode(uint8_t new_yaw_mode) yaw_initialised = true; } break; - case YAW_TOY: - yaw_initialised = true; - break; case YAW_LOOK_AHEAD: if( ap.home_is_set ) { yaw_initialised = true; } break; + case YAW_TOY: + yaw_initialised = true; + break; + case YAW_RESETTOARMEDYAW: + nav_yaw = ahrs.yaw_sensor; // store current yaw so we can start rotating back to correct one + yaw_initialised = true; + break; } // if initialisation has been successful update the yaw mode @@ -1450,11 +1452,6 @@ void update_yaw_mode(void) } break; - case YAW_RESETTOARMEDYAW: - nav_yaw = get_yaw_slew(nav_yaw, initial_simple_bearing, AUTO_YAW_SLEW_RATE); - get_stabilize_yaw(nav_yaw); - break; - case YAW_LOOK_AT_NEXT_WP: // point towards next waypoint (no pilot input accepted) // we don't use wp_bearing because we don't want the copter to turn too much during flight @@ -1517,6 +1514,12 @@ void update_yaw_mode(void) get_stabilize_yaw(nav_yaw); break; #endif + + case YAW_RESETTOARMEDYAW: + // changes yaw to be same as when quad was armed + nav_yaw = get_yaw_slew(nav_yaw, initial_simple_bearing, AUTO_YAW_SLEW_RATE); + get_stabilize_yaw(nav_yaw); + break; } } diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 0f6433c8db..62f008531d 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -91,13 +91,6 @@ static void do_aux_switch_function(int8_t ch_function, bool ch_flag) } switch(tmp_function) { - case AUX_SWITCH_RESETTOARMEDYAW: - if(ch_flag) - set_yaw_mode(YAW_RESETTOARMEDYAW); - else - set_yaw_mode(YAW_HOLD); - break; - case AUX_SWITCH_FLIP: // flip if switch is on, positive throttle and we're actually flying if(ch_flag && g.rc_3.control_in >= 0 && ap.takeoff_complete) { @@ -198,6 +191,13 @@ static void do_aux_switch_function(int8_t ch_function, bool ch_flag) fence.enable(ch_flag); break; #endif + case AUX_SWITCH_RESETTOARMEDYAW: + if (ch_flag) { + set_yaw_mode(YAW_RESETTOARMEDYAW); + }else{ + set_yaw_mode(YAW_HOLD); + } + break; } } diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index 28df6ffcc3..442e7a3983 100644 --- a/ArduCopter/defines.h +++ b/ArduCopter/defines.h @@ -24,7 +24,7 @@ #define YAW_LOOK_AT_HEADING 6 // point towards a particular angle (not pilot input accepted) #define YAW_LOOK_AHEAD 7 // WARNING! CODE IN DEVELOPMENT NOT PROVEN #define YAW_TOY 8 // THOR This is the Yaw mode -#define YAW_RESETTOARMEDYAW 9 +#define YAW_RESETTOARMEDYAW 9 // point towards heading at time motors were armed #define ROLL_PITCH_STABLE 0 // pilot input roll, pitch angles #define ROLL_PITCH_ACRO 1 // pilot inputs roll, pitch rotation rates