AP_Math: added location_update() and location_offset() functions

these will be used by AHRS dead reckoning

libraries/AP_Math/AP_Math.h

@@ -48,5 +48,11 @@ bool location_passed_point(struct Location &location,
 						   struct Location &point1, 			   
 						   struct Location &point2);
 
+//  extrapolate latitude/longitude given bearing and distance
+void location_update(struct Location *loc, float bearing, float distance);
+
+// extrapolate latitude/longitude given distances north and east
+void location_offset(struct Location *loc, float ofs_north, float ofs_east);
+
 #endif // AP_MATH_H
 

libraries/AP_Math/examples/location/location.pde

@@ -83,6 +83,59 @@ static void test_passed_waypoint(void)
 	Serial.println("waypoint tests OK");
 }
 
+static void test_one_offset(struct Location &loc, 
+                            float ofs_north, float ofs_east,
+                            float dist, float bearing)
+{
+    struct Location loc2;
+    float dist2, bearing2;
+
+    loc2 = loc;
+    uint32_t t1 = micros();
+    location_offset(&loc2, ofs_north, ofs_east);
+    Serial.printf("location_offset took %u usec\n", 
+                  micros() - t1);
+    dist2 = get_distance(&loc, &loc2);
+    bearing2 = get_bearing_cd(&loc, &loc2) * 0.01;
+    float brg_error = bearing2-bearing;
+    if (brg_error > 180) {
+        brg_error -= 360;
+    } else if (brg_error < -180) {
+        brg_error += 360;
+    }
+
+    if (abs(dist - dist2) > 1.0 ||
+        brg_error > 1.0) {
+        Serial.printf("Failed offset test brg_error=%f dist_error=%f\n",
+                      brg_error, dist-dist2);
+    }
+}
+
+static const struct {
+    float ofs_north, ofs_east, distance, bearing;
+} test_offsets[] = {
+    { 1000, 1000,  sqrt(2.0)*1000, 45 },
+    { 1000, -1000, sqrt(2.0)*1000, -45 },
+    { 1000, 0,     1000, 0 },
+    { 0, 1000,     1000, 90 },
+};
+
+static void test_offset(void)
+{
+    struct Location loc;
+
+    loc.lat = -35*1.0e7;
+    loc.lng = 149*1.0e7;
+    
+	for (uint8_t i=0; i<ARRAY_LENGTH(test_offsets); i++) {
+        test_one_offset(loc, 
+                        test_offsets[i].ofs_north, 
+                        test_offsets[i].ofs_east, 
+                        test_offsets[i].distance,
+                        test_offsets[i].bearing);
+    }
+}
+
 /*
   polygon tests
  */
@@ -90,6 +143,7 @@ void setup(void)
 {
     Serial.begin(115200);
     test_passed_waypoint();
+    test_offset();
 }
 
 void

libraries/AP_Math/location.cpp

@@ -24,6 +24,9 @@
 #include <FastSerial.h>
 #include "AP_Math.h"
 
+// radius of earth in meters
+#define RADIUS_OF_EARTH 6378100
+
 static float longitude_scale(const struct Location *loc)
 {
 	static uint32_t last_lat;
@@ -109,3 +112,37 @@ bool location_passed_point(struct Location &location,
 	return false;	
 }
 
+/*
+  extrapolate latitude/longitude given bearing and distance
+  thanks to http://www.movable-type.co.uk/scripts/latlong.html
+
+  This function is precise, but costs about 1.7 milliseconds on an AVR2560
+*/
+void location_update(struct Location *loc, float bearing, float distance)
+{
+	float lat1 = radians(loc->lat*1.0e-7);
+	float lon1 = radians(loc->lng*1.0e-7);
+	float brng = radians(bearing);
+	float dr = distance/RADIUS_OF_EARTH;
+
+	float lat2 = asin(sin(lat1)*cos(dr) +
+                      cos(lat1)*sin(dr)*cos(brng));
+    float lon2 = lon1 + atan2(sin(brng)*sin(dr)*cos(lat1), 
+                              cos(dr)-sin(lat1)*sin(lat2));
+    loc->lat = degrees(lat2)*1.0e7;
+    loc->lng = degrees(lon2)*1.0e7;
+}
+
+/*
+  extrapolate latitude/longitude given distances north and east
+  This function costs about 80 usec on an AVR2560
+*/
+void location_offset(struct Location *loc, float ofs_north, float ofs_east)
+{
+    if (ofs_north != 0 || ofs_east != 0) {
+        float dlat = ofs_north * 89.831520982;
+        float dlng = (ofs_east * 89.831520982) / longitude_scale(loc);
+        loc->lat += dlat;
+        loc->lng += dlng;
+    }
+}