From 4af392290b5a68782e6bc222f06932843192aaaa Mon Sep 17 00:00:00 2001 From: Jason Short Date: Fri, 13 Jul 2012 19:29:36 -0700 Subject: [PATCH] Circle_WP: Fix for Yaw toward center, fix for transit to Circle WP from a distance --- ArduCopter/ArduCopter.pde | 27 ++++++++++++++------------- 1 file changed, 14 insertions(+), 13 deletions(-) diff --git a/ArduCopter/ArduCopter.pde b/ArduCopter/ArduCopter.pde index 119e00a6ea..10a5061af2 100644 --- a/ArduCopter/ArduCopter.pde +++ b/ArduCopter/ArduCopter.pde @@ -1938,7 +1938,6 @@ static void update_navigation() break; case CIRCLE: - yaw_tracking = MAV_ROI_WPNEXT; wp_control = CIRCLE_MODE; // calculates desired Yaw @@ -2353,12 +2352,17 @@ static void update_nav_wp() next_WP.lng = circle_WP.lng + (g.loiter_radius * 100 * cos(1.57 - circle_angle) * scaleLongUp); next_WP.lat = circle_WP.lat + (g.loiter_radius * 100 * sin(1.57 - circle_angle)); - // calc the lat and long error to the target - calc_location_error(&next_WP); - // use error as the desired rate towards the target // nav_lon, nav_lat is calculated - calc_loiter(long_error, lat_error); + + if(wp_distance > 400){ + calc_nav_rate(calc_desired_speed(g.waypoint_speed_max, true)); + }else{ + // calc the lat and long error to the target + calc_location_error(&next_WP); + + calc_loiter(long_error, lat_error); + } //CIRCLE: angle:29, dist:0, lat:400, lon:242 @@ -2400,23 +2404,20 @@ static void update_nav_wp() static void update_auto_yaw() { - // If we Loiter, don't start Yawing, allow Yaw control - if(wp_control == LOITER_MODE) - return; - - if(control_mode == CIRCLE){ + if(wp_control == CIRCLE_MODE){ + //trace("CIRCLE mode") auto_yaw = get_bearing(¤t_loc, &circle_WP); + }else if(wp_control == LOITER_MODE){ + // just hold nav_yaw + }else if(yaw_tracking == MAV_ROI_LOCATION){ - // this tracks a location so the copter is always pointing towards it. auto_yaw = get_bearing(¤t_loc, &target_WP); }else if(yaw_tracking == MAV_ROI_WPNEXT){ // Point towards next WP auto_yaw = target_bearing; } - //Serial.printf("auto_yaw %d ", auto_yaw); - // MAV_ROI_NONE = basic Yaw hold } /* MAV_ROI_NONE=0, No region of interest. |