Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-10 09:58:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

Circle_WP: Fix for Yaw toward center, fix for transit to Circle WP from a distance

Browse Source
This commit is contained in:
Jason Short 2012-07-13 19:29:36 -07:00
parent 7b746cf5e1
commit 4af392290b
1 changed files with 14 additions and 13 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
ArduCopter/ArduCopter.pde
View File

@ -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(&current_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(&current_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. |