From e9d640ced8b9e8bac95154704c52d4e8f898e3f0 Mon Sep 17 00:00:00 2001 From: Dr Gareth Owen Date: Mon, 20 May 2013 20:26:39 +0100 Subject: [PATCH] added ch7 option to reset yaw back to what it was when quad was armed --- ArduCopter/ArduCopter.pde | 7 +++++++ ArduCopter/Parameters.pde | 4 ++-- ArduCopter/control_modes.pde | 7 +++++++ ArduCopter/defines.h | 4 ++-- 4 files changed, 18 insertions(+), 4 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 0985ed62b5..688fad44c6 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1376,6 +1376,8 @@ 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; @@ -1448,6 +1450,11 @@ 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 diff --git a/ArduCopter/Parameters.pde b/ArduCopter/Parameters.pde index db9c573a0a..b14740734d 100644 --- a/ArduCopter/Parameters.pde +++ b/ArduCopter/Parameters.pde @@ -409,14 +409,14 @@ const AP_Param::Info var_info[] PROGMEM = { // @Param: CH7_OPT // @DisplayName: Channel 7 option // @Description: Select which function if performed when CH7 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:Sonar + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:Sonar, 12:ResetToArmedYaw // @User: Standard GSCALAR(ch7_option, "CH7_OPT", CH7_OPTION), // @Param: CH8_OPT // @DisplayName: Channel 8 option // @Description: Select which function if performed when CH8 is above 1800 pwm - // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:Sonar + // @Values: 0:Do Nothing, 2:Flip, 3:Simple Mode, 4:RTL, 5:Save Trim, 7:Save WP, 8:Multi Mode, 9:Camera Trigger, 10:Sonar, 12:ResetToArmedYaw // @User: Standard GSCALAR(ch8_option, "CH8_OPT", CH8_OPTION), diff --git a/ArduCopter/control_modes.pde b/ArduCopter/control_modes.pde index 7e156b186f..ef038c8742 100644 --- a/ArduCopter/control_modes.pde +++ b/ArduCopter/control_modes.pde @@ -91,6 +91,13 @@ 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) { diff --git a/ArduCopter/defines.h b/ArduCopter/defines.h index b18037ea1b..28df6ffcc3 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 ROLL_PITCH_STABLE 0 // pilot input roll, pitch angles #define ROLL_PITCH_ACRO 1 // pilot inputs roll, pitch rotation rates @@ -61,7 +61,7 @@ #define AUX_SWITCH_CAMERA_TRIGGER 9 // trigger camera servo or relay #define AUX_SWITCH_SONAR 10 // allow enabling or disabling sonar in flight which helps avoid surface tracking when you are far above the ground #define AUX_SWITCH_FENCE 11 // allow enabling or disabling fence in flight - +#define AUX_SWITCH_RESETTOARMEDYAW 12 // changes yaw to be same as when quad was armed // Frame types