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

ACM: Yaw Look Ahead Changes

Browse Source
This commit is contained in:
Robert Lefebvre 2012-12-04 13:26:41 -05:00
parent 47529e9454
commit 315a7e8000
2 changed files with 17 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
ArduCopter/ArduCopter.pde
View File

@ -429,6 +429,7 @@ static struct AP_System{
// updated after GPS read - 5-10hz
static int16_t lon_speed; // expressed in cm/s. positive numbers mean moving east
static int16_t lat_speed; // expressed in cm/s. positive numbers when moving north
static int16_t ground_bearing; // expressed in centidegrees
static int16_t desired_climb_rate;
@ -804,8 +805,6 @@ static int8_t alt_change_flag;
////////////////////////////////////////////////////////////////////////////////
// The Commanded Yaw from the autopilot.
static int32_t nav_yaw;
// Commanded Yaw to automatically look ahead.
static int32_t look_ahead_yaw;
// A speed governer for Yaw control - limits the rate the quad can be turned by the autopilot
static int32_t auto_yaw;
static uint8_t yaw_timer;
@ -1508,13 +1507,16 @@ void update_yaw_mode(void)
case YAW_LOOK_AHEAD:
if (g_gps->ground_speed > 300){ // Speed in cm/s. Create deadband of 3m/s.
look_ahead_yaw = (float)((g_gps->ground_speed - 300)/YAW_LOOK_AHEAD_RATE * (g_gps->ground_course - nav_yaw));
look_ahead_yaw = constrain ((look_ahead_yaw + g.rc_4.control_in), -4500, 4500);
} else {
look_ahead_yaw = g.rc_4.control_in;
// Commanded Yaw to automatically look ahead.
if (g_gps->ground_speed > YAW_LOOK_AHEAD_RATE){ // Speed in cm/s.
nav_yaw += constrain(wrap_180(ground_bearing - nav_yaw), -60, 60); // 40 deg a second
nav_yaw = wrap_360(nav_yaw);
get_stabilize_yaw(nav_yaw + g.rc_4.control_in); // Allow pilot to "skid" around corners up to 45°
} else {
nav_yaw += g.rc_4.control_in * g.acro_p * G_Dt;
nav_yaw = wrap_360(nav_yaw);
get_stabilize_yaw(nav_yaw);
}
get_yaw_rate_stabilized_ef(look_ahead_yaw);
break;
}
}

7
ArduCopter/navigation.pde
View File

@ -46,6 +46,9 @@ static void update_navigation()
// calculate velocity
calc_velocity_and_position();
// calculate ground bearing
calc_ground_bearing();
// calculate distance, angles to target
calc_distance_and_bearing();
@ -127,6 +130,10 @@ static void calc_velocity_and_position(){
last_gps_latitude = g_gps->latitude;
}
static void calc_ground_bearing(){
ground_bearing = atan2( lat_speed , lon_speed ) * DEGX100;
}
//****************************************************************
// Function that will calculate the desired direction to fly and distance
//****************************************************************