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

better crosstrack

This commit is contained in:
Michael Oborne 2011-10-29 12:54:13 +08:00
parent f70868348c
commit 374f8cbcd0
3 changed files with 11 additions and 5 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
ArduPlane/Attitude.pde
View File

@ -190,11 +190,17 @@ static void calc_nav_pitch()
static void calc_nav_roll()
{
float psi_dot_cmd = g.pidNavRoll.kP() * (bearing_error / 100.0);
//float psi_dot_cmd = g.pidNavRoll.kP() * (bearing_error / 100.0);
nav_roll = (100 * ToDeg(atan((psi_dot_cmd * ((float)g_gps->ground_speed)) / 981.0)));
//nav_roll = (100 * ToDeg(atan((psi_dot_cmd * ((float)g_gps->ground_speed)) / 981.0)));
//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);
long psi_dot_cmd = g.pidNavRoll.kP() * bearing_error;
nav_roll = (100 * ToDeg(atan((psi_dot_cmd * g_gps->ground_speed) / 9810000.0)));
// printf("nvrl %ld err %ld psi %ld gps gs %ld COG %ld\n",nav_roll,bearing_error,psi_dot_cmd,g_gps->ground_speed,g_gps->ground_course);
Vector3f omega;
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);
scaleLongUp = 1.0f/cos(rads);
// this is handy for the groundstation
wp_totalDistance = get_distance(&prev_WP, &next_WP);
wp_totalDistance = get_distance(&current_loc, &next_WP);
wp_distance = wp_totalDistance;
target_bearing = get_bearing(&prev_WP, &next_WP);
target_bearing = get_bearing(&current_loc, &next_WP);
nav_bearing = target_bearing;
// to check if we have missed the WP

2
ArduPlane/navigation.pde
View File

@ -183,7 +183,7 @@ static void update_crosstrack(void)
static void reset_crosstrack()
{
crosstrack_bearing = get_bearing(&current_loc, &next_WP); // Used for track following
crosstrack_bearing = get_bearing(&prev_WP, &next_WP); // Used for track following
}
static long get_distance(struct Location *loc1, struct Location *loc2)