Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-26 18:48:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

Added Nav_bearing, just like APlane

Browse Source 
added fix for LOITER in missions so we don't Yaw in circles
This commit is contained in:
Jason Short 2011-12-30 23:19:11 -08:00
parent 758a5240f2
commit 09db32ee02
1 changed files with 9 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduCopter/ArduCopter.pde
View File

@ -366,6 +366,8 @@ static bool nav_ok;
static const float radius_of_earth = 6378100; // meters static const float radius_of_earth = 6378100; // meters
static const float gravity = 9.81; // meters/ sec^2 static const float gravity = 9.81; // meters/ sec^2
static int32_t target_bearing; // deg * 100 : 0 to 360 location of the plane to the target static int32_t target_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static int32_t nav_bearing; // deg * 100 : 0 to 360 location of the plane to the target
static int32_t home_bearing; // used to track difference in angle static int32_t home_bearing; // used to track difference in angle
static byte wp_control; // used to control - navgation or loiter static byte wp_control; // used to control - navgation or loiter
@ -1762,7 +1764,8 @@ static void update_nav_wp()
// use error as the desired rate towards the target // use error as the desired rate towards the target
calc_nav_rate(g.waypoint_speed_max); calc_nav_rate(g.waypoint_speed_max);
// rotate pitch and roll to the copter frame of reference // rotate pitch and roll to the copter frame of reference
calc_nav_pitch_roll(); //calc_nav_pitch_roll();
calc_loiter_pitch_roll();
}else if(wp_control == NO_NAV_MODE){ }else if(wp_control == NO_NAV_MODE){
nav_roll = 0; nav_roll = 0;
@ -1772,11 +1775,16 @@ static void update_nav_wp()
static void update_auto_yaw() static void update_auto_yaw()
{ {
// If we Loiter, don't start Yawing, allow Yaw control
if(wp_control == LOITER_MODE)
return;
// this tracks a location so the copter is always pointing towards it. // this tracks a location so the copter is always pointing towards it.
if(yaw_tracking == MAV_ROI_LOCATION){ if(yaw_tracking == MAV_ROI_LOCATION){
auto_yaw = get_bearing(&current_loc, &target_WP); auto_yaw = get_bearing(&current_loc, &target_WP);
}else if(yaw_tracking == MAV_ROI_WPNEXT){ }else if(yaw_tracking == MAV_ROI_WPNEXT){
// Point towards next WP
auto_yaw = target_bearing; auto_yaw = target_bearing;
} }
// MAV_ROI_NONE = basic Yaw hold // MAV_ROI_NONE = basic Yaw hold