From 6ee9f1ae9737451711e9c8c752fc2ef4d1e9b3dd Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Jul 2012 09:20:12 +1000 Subject: [PATCH] APM: use new location functions --- ArduPlane/ArduPlane.pde | 5 ----- ArduPlane/commands.pde | 9 --------- ArduPlane/navigation.pde | 19 ------------------- 3 files changed, 33 deletions(-) diff --git a/ArduPlane/ArduPlane.pde b/ArduPlane/ArduPlane.pde index 7852434b72..82961fd826 100644 --- a/ArduPlane/ArduPlane.pde +++ b/ArduPlane/ArduPlane.pde @@ -362,11 +362,6 @@ static bool GPS_light; // This approximation makes calculations integer and it's easy to read static const float t7 = 10000000.0; // 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 // before recording our home position (and executing a ground start if we booted with an air start) static byte ground_start_count = 5; diff --git a/ArduPlane/commands.pde b/ArduPlane/commands.pde index 3d16189e9d..6d50e81e77 100644 --- a/ArduPlane/commands.pde +++ b/ArduPlane/commands.pde @@ -157,10 +157,6 @@ static void set_next_WP(struct Location *wp) loiter_sum = 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 wp_totalDistance = get_distance(¤t_loc, &next_WP); wp_distance = wp_totalDistance; @@ -191,11 +187,6 @@ static void set_guided_WP(void) target_altitude = current_loc.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 wp_totalDistance = get_distance(¤t_loc, &next_WP); wp_distance = wp_totalDistance; diff --git a/ArduPlane/navigation.pde b/ArduPlane/navigation.pde index df95caab05..91edcac7c0 100644 --- a/ArduPlane/navigation.pde +++ b/ArduPlane/navigation.pde @@ -224,22 +224,3 @@ static void reset_crosstrack() 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; -}