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

APM: use new location functions

This commit is contained in:
Andrew Tridgell 2012-07-04 09:20:12 +10:00
parent dd200cba31
commit 6ee9f1ae97
3 changed files with 0 additions and 33 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
ArduPlane/ArduPlane.pde
View File

@ -362,11 +362,6 @@ static bool GPS_light;
// This approximation makes calculations integer and it's easy to read // This approximation makes calculations integer and it's easy to read
static const float t7 = 10000000.0; static const float t7 = 10000000.0;
// We use atan2 and other trig techniques to calaculate angles // We use atan2 and other trig techniques to calaculate angles
// We need to scale the longitude up to make these calcs work
// to account for decreasing distance between lines of longitude away from the equator
static float scaleLongUp = 1;
// Sometimes we need to remove the scaling for distance calcs
static float scaleLongDown = 1;
// A counter used to count down valid gps fixes to allow the gps estimate to settle // A counter used to count down valid gps fixes to allow the gps estimate to settle
// before recording our home position (and executing a ground start if we booted with an air start) // before recording our home position (and executing a ground start if we booted with an air start)
static byte ground_start_count = 5; static byte ground_start_count = 5;

9
ArduPlane/commands.pde
View File

@ -157,10 +157,6 @@ static void set_next_WP(struct Location *wp)
loiter_sum = 0; loiter_sum = 0;
loiter_total = 0; loiter_total = 0;
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (fabs((float)next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = 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(&current_loc, &next_WP);
wp_distance = wp_totalDistance; wp_distance = wp_totalDistance;
@ -191,11 +187,6 @@ static void set_guided_WP(void)
target_altitude = current_loc.alt; target_altitude = current_loc.alt;
offset_altitude = next_WP.alt - prev_WP.alt; offset_altitude = next_WP.alt - prev_WP.alt;
// this is used to offset the shrinking longitude as we go towards the poles
float rads = (abs(next_WP.lat)/t7) * 0.0174532925;
scaleLongDown = 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(&current_loc, &next_WP);
wp_distance = wp_totalDistance; wp_distance = wp_totalDistance;

19
ArduPlane/navigation.pde
View File

@ -224,22 +224,3 @@ static void reset_crosstrack()
crosstrack_bearing = get_bearing(&prev_WP, &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)
{
if(loc1->lat == 0 || loc1->lng == 0)
return -1;
if(loc2->lat == 0 || loc2->lng == 0)
return -1;
float dlat = (float)(loc2->lat - loc1->lat);
float dlong = ((float)(loc2->lng - loc1->lng)) * scaleLongDown;
return sqrt(sq(dlat) + sq(dlong)) * .01113195;
}
static long get_bearing(struct Location *loc1, struct Location *loc2)
{
long off_x = loc2->lng - loc1->lng;
long off_y = (loc2->lat - loc1->lat) * scaleLongUp;
long bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
if (bearing < 0) bearing += 36000;
return bearing;
}