From 0aabd4efe299e2b2f0ab2ea72b42fe742667cbef Mon Sep 17 00:00:00 2001 From: Jason Short Date: Sun, 1 Jul 2012 13:38:53 -0700 Subject: [PATCH] Yaw mode: Improved hybrid Yaw mode: - uses rate controller rather than as fast as possible - waits 1.5 seconds before entering hold - bounce free --- ArduCopter/ArduCopter.pde | 25 ++++++++++++++++++------- ArduCopter/Attitude.pde | 15 ++++++++------- 2 files changed, 26 insertions(+), 14 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 23d3381521..858a794dd2 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -801,6 +801,8 @@ static int32_t nav_yaw; // A speed governer for Yaw control - limits the rate the quad can be turned by the autopilot static int32_t auto_yaw; // Used to manage the Yaw hold capabilities - +static bool yaw_stopped; +static uint8_t yaw_timer; // Options include: no tracking, point at next wp, or at a target static byte yaw_tracking = MAV_ROI_WPNEXT; // In AP Mission scripting we have a fine level of control for Yaw @@ -1107,7 +1109,6 @@ static void medium_loop() //update_altitude(); //#endif alt_sensor_flag = true; - break; // This case deals with sending high rate telemetry @@ -1484,12 +1485,22 @@ void update_yaw_mode(void) break; case YAW_HOLD: - // calcualte new nav_yaw offset - if (control_mode <= STABILIZE){ - nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in); + if(g.rc_4.control_in != 0){ + g.rc_4.servo_out = get_acro_yaw(g.rc_4.control_in); + yaw_stopped = false; + yaw_timer = 150; + }else if (!yaw_stopped){ + g.rc_4.servo_out = get_acro_yaw(0); + yaw_timer--; + if(yaw_timer == 0){ + yaw_stopped = true; + nav_yaw = ahrs.yaw_sensor; + } }else{ - nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, 1); + nav_yaw = get_nav_yaw_offset(g.rc_4.control_in, g.rc_3.control_in); + g.rc_4.servo_out = get_stabilize_yaw(nav_yaw); } + return; break; case YAW_LOOK_AT_HOME: @@ -2125,9 +2136,9 @@ adjust_altitude() manual_boost = (g.rc_3.control_in - g.throttle_min) - THROTTLE_ADJUST; manual_boost = max(-THROTTLE_ADJUST, manual_boost); - }else if (g.rc_3.control_in >= (MAXIMUM_THROTTLE - THROTTLE_ADJUST)){ + }else if (g.rc_3.control_in >= (g.throttle_max - THROTTLE_ADJUST)){ // we add 0 to 100 PWM to hover - manual_boost = g.rc_3.control_in - (MAXIMUM_THROTTLE - THROTTLE_ADJUST); + manual_boost = g.rc_3.control_in - (g.throttle_max - THROTTLE_ADJUST); manual_boost = min(THROTTLE_ADJUST, manual_boost); }else { diff --git a/ArduCopter/Attitude.pde b/ArduCopter/Attitude.pde index 7c9b2f7b2a..0c4b23de4d 100644 --- a/ArduCopter/Attitude.pde +++ b/ArduCopter/Attitude.pde @@ -253,8 +253,15 @@ get_rate_yaw(int32_t target_rate) output = p+i+d; - // constrain output +#if FRAME_CONFIG == HELI_FRAME output = constrain(output, -4500, 4500); +#else + // output control: + int16_t yaw_limit = 1900 + abs(g.rc_4.control_in); + + // constrain output + output = constrain(output, -yaw_limit, yaw_limit); +#endif #if LOGGING_ENABLED == ENABLED static int8_t log_counter = 0; // used to slow down logging of PID values to dataflash @@ -407,22 +414,16 @@ get_nav_yaw_offset(int yaw_input, int reset) return ahrs.yaw_sensor; }else{ -#if ALTERNATIVE_YAW_MODE == ENABLED - _yaw = nav_yaw + (yaw_input / 50); - return wrap_360(_yaw); -#else // re-define nav_yaw if we have stick input if(yaw_input != 0){ // set nav_yaw + or - the current location _yaw = yaw_input + ahrs.yaw_sensor; // we need to wrap our value so we can be 0 to 360 (*100) return wrap_360(_yaw); - }else{ // no stick input, lets not change nav_yaw return nav_yaw; } -#endif } }