Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

APM track following change

This commit is contained in:
Michael Oborne 2011-10-28 12:05:22 +08:00
parent b594bd7f69
commit 357f299d61
3 changed files with 10 additions and 17 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
ArduPlane/Attitude.pde
View File

@ -190,18 +190,11 @@ static void calc_nav_pitch()
static void calc_nav_roll() static void calc_nav_roll()
{ {
float psi_dot_cmd = g.pidNavRoll.kP() * (bearing_error / 100.0);
// Adjust gain based on ground speed - We need lower nav gain going in to a headwind, etc. nav_roll = (100 * ToDeg(atan((psi_dot_cmd * ((float)g_gps->ground_speed)) / 981.0)));
// This does not make provisions for wind speed in excess of airframe speed
nav_gain_scaler = (float)g_gps->ground_speed / (STANDARD_SPEED * 100.0);
nav_gain_scaler = constrain(nav_gain_scaler, 0.2, 1.4);
// negative error = left turn //printf("nvrl %ld err %ld psi %f gps gs %ld COG %ld\n",nav_roll,bearing_error,psi_dot_cmd,g_gps->ground_speed,g_gps->ground_course);
// positive error = right turn
// Calculate the required roll of the plane
// ----------------------------------------
nav_roll = g.pidNavRoll.get_pid(bearing_error, dTnav, nav_gain_scaler); //returns desired bank angle in degrees*100
nav_roll = constrain(nav_roll, -g.roll_limit.get(), g.roll_limit.get());
Vector3f omega; Vector3f omega;
omega = dcm.get_gyro(); omega = dcm.get_gyro();

4
ArduPlane/commands.pde
View File

@ -180,9 +180,9 @@ static void set_next_WP(struct Location *wp)
scaleLongDown = cos(rads); scaleLongDown = cos(rads);
scaleLongUp = 1.0f/cos(rads); scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation // this is handy for the groundstation
wp_totalDistance = get_distance(&current_loc, &next_WP); wp_totalDistance = get_distance(&prev_WP, &next_WP);
wp_distance = wp_totalDistance; wp_distance = wp_totalDistance;
target_bearing = get_bearing(&current_loc, &next_WP); target_bearing = get_bearing(&prev_WP, &next_WP);
nav_bearing = target_bearing; nav_bearing = target_bearing;
// to check if we have missed the WP // to check if we have missed the WP

6
ArduPlane/navigation.pde
View File

@ -75,15 +75,15 @@ static void calc_airspeed_errors()
} }
static void calc_bearing_error() static void calc_bearing_error()
{ {/*
if(takeoff_complete == true || g.compass_enabled == true) { if(takeoff_complete == true || g.compass_enabled == true) {
bearing_error = nav_bearing - dcm.yaw_sensor; bearing_error = nav_bearing - dcm.yaw_sensor;
} else { } else {
*/
// TODO: we need to use the Yaw gyro for in between GPS reads, // TODO: we need to use the Yaw gyro for in between GPS reads,
// maybe as an offset from a saved gryo value. // maybe as an offset from a saved gryo value.
bearing_error = nav_bearing - g_gps->ground_course; bearing_error = nav_bearing - g_gps->ground_course;
} // }
bearing_error = wrap_180(bearing_error); bearing_error = wrap_180(bearing_error);
} }