Math: added location functions to math library

these do common calculations on struct Location

libraries/AP_Math/AP_Math.h

@@ -29,4 +29,20 @@ float safe_sqrt(float v);
 // rotation to an existing rotation of a sensor such as the compass
 enum Rotation rotation_combination(enum Rotation r1, enum Rotation r2, bool *found = NULL);
 
-#endif
+// return distance in meters between two locations
+int32_t get_distance(const struct Location *loc1, const struct Location *loc2);
+
+// return bearing in centi-degrees between two locations
+int32_t get_bearing(const struct Location *loc1, const struct Location *loc2);
+
+// see if location is past a line perpendicular to 
+// the line between point1 and point2. If point1 is 
+// our previous waypoint and point2 is our target waypoint
+// then this function returns true if we have flown past
+// the target waypoint
+bool location_passed_point(struct Location &location, 
+						   struct Location &point1, 			   
+						   struct Location &point2);
+
+#endif // AP_MATH_H
+

libraries/AP_Math/examples/location/Makefile

@@ -0,0 +1,4 @@
+include ../../../AP_Common/Arduino.mk
+
+sitl:
+	make -f ../../../../libraries/Desktop/Desktop.mk

libraries/AP_Math/examples/location/location.pde

@@ -0,0 +1,101 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+//
+// Unit tests for the AP_Math polygon code
+//
+
+#include <FastSerial.h>
+#include <AP_Common.h>
+#include <AP_Math.h>
+
+#ifdef DESKTOP_BUILD
+// all of this is needed to build with SITL
+#include <DataFlash.h>
+#include <APM_RC.h>
+#include <GCS_MAVLink.h>
+#include <Arduino_Mega_ISR_Registry.h>
+#include <AP_PeriodicProcess.h>
+#include <AP_ADC.h>
+#include <AP_Baro.h>
+#include <AP_Compass.h>
+#include <AP_GPS.h>
+#include <Filter.h>
+#include <SITL.h>
+#include <I2C.h>
+#include <SPI.h>
+#include <AP_Declination.h>
+Arduino_Mega_ISR_Registry isr_registry;
+AP_Baro_BMP085_HIL      barometer;
+AP_Compass_HIL     compass;
+SITL sitl;
+BetterStream *mavlink_comm_0_port;
+BetterStream *mavlink_comm_1_port;
+mavlink_system_t mavlink_system;
+#endif
+
+FastSerialPort(Serial, 0);
+
+static const struct {
+	Vector2f wp1, wp2, location;
+	bool passed;
+} test_points[] = {
+	{ Vector2f(-35.3647759314918, 149.16265692810987), 
+	  Vector2f(-35.36279922658029, 149.16352169591426), 
+	  Vector2f(-35.36214956969903, 149.16461410046492), true },
+	{ Vector2f(-35.36438601157189, 149.16613916088568),
+	  Vector2f(-35.364432558610254, 149.16287313113048),
+	  Vector2f(-35.36491510034746, 149.16365837225004), false },
+	{ Vector2f(0, 0),
+	  Vector2f(0, 1),
+	  Vector2f(0, 2), true },
+	{ Vector2f(0, 0),
+	  Vector2f(0, 2),
+	  Vector2f(0, 1), false },
+	{ Vector2f(0, 0),
+	  Vector2f(1, 0),
+	  Vector2f(2, 0), true },
+	{ Vector2f(0, 0),
+	  Vector2f(2, 0),
+	  Vector2f(1, 0), false },
+	{ Vector2f(0, 0),
+	  Vector2f(-1, 1),
+	  Vector2f(-2, 2), true },
+};
+
+#define ARRAY_LENGTH(x) (sizeof((x))/sizeof((x)[0]))
+
+static struct Location location_from_point(Vector2f pt)
+{
+	struct Location loc = {0};
+	loc.lat = pt.x * 1.0e7;
+	loc.lng = pt.y * 1.0e7;
+	return loc;
+}
+
+static void test_passed_waypoint(void)
+{
+	Serial.println("waypoint tests starting");
+	for (uint8_t i=0; i<ARRAY_LENGTH(test_points); i++) {
+		struct Location loc = location_from_point(test_points[i].location);
+		struct Location wp1 = location_from_point(test_points[i].wp1);
+		struct Location wp2 = location_from_point(test_points[i].wp2);
+		if (location_passed_point(loc, wp1, wp2) != test_points[i].passed) {
+			Serial.printf("Failed waypoint test %u\n", (unsigned)i);
+			return;
+		}
+	}
+	Serial.println("waypoint tests OK");
+}
+
+/*
+  polygon tests
+ */
+void setup(void)
+{
+    Serial.begin(115200);
+    test_passed_waypoint();
+}
+
+void
+loop(void)
+{
+}

libraries/AP_Math/location.cpp

@@ -0,0 +1,96 @@
+/// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+/*
+ * location.cpp
+ * Copyright (C) Andrew Tridgell 2011
+ *
+ * This file is free software: you can redistribute it and/or modify it
+ * under the terms of the GNU General Public License as published by the
+ * Free Software Foundation, either version 3 of the License, or
+ * (at your option) any later version.
+ *
+ * This file is distributed in the hope that it will be useful, but
+ * WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.
+ * See the GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License along
+ * with this program.  If not, see <http://www.gnu.org/licenses/>.
+ */
+
+/*
+  this module deals with calculations involving struct Location
+ */
+
+#include <FastSerial.h>
+#include "AP_Math.h"
+
+static float longitude_scale(const struct Location *loc)
+{
+	static uint32_t last_lat;
+	static float scale = 1.0;
+	if (abs(last_lat - loc->lat) < 100000) {
+		// we are within 0.01 degrees (about 1km) of the 
+		// same latitude. We can avoid the cos() and return
+		// the same scale factor.  
+		return scale;
+	}
+	scale = cos((fabs((float)loc->lat)/1.0e7) * 0.0174532925);
+	return scale;
+}
+
+
+
+// return distance in meters to between two locations, or -1
+// if one of the locations is invalid
+int32_t get_distance(const struct Location *loc1, const 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)) * longitude_scale(loc2);
+	return sqrt(sq(dlat) + sq(dlong)) * .01113195;
+}
+
+// return bearing in centi-degrees between two locations
+int32_t get_bearing(const struct Location *loc1, const struct Location *loc2)
+{
+	int32_t off_x = loc2->lng - loc1->lng;
+	int32_t off_y = (loc2->lat - loc1->lat) / longitude_scale(loc2);
+	int32_t bearing = 9000 + atan2(-off_y, off_x) * 5729.57795;
+	if (bearing < 0) bearing += 36000;
+	return bearing;
+}
+
+// see if location is past a line perpendicular to 
+// the line between point1 and point2. If point1 is 
+// our previous waypoint and point2 is our target waypoint
+// then this function returns true if we have flown past
+// the target waypoint
+bool location_passed_point(struct Location &location, 
+			   struct Location &point1, 			   
+			   struct Location &point2)
+{
+	// the 3 points form a triangle. If the angle between lines
+	// point1->point2 and location->point2 is greater than 90
+	// degrees then we have passed the waypoint
+	Vector2f loc1(location.lat, location.lng);
+	Vector2f pt1(point1.lat, point1.lng);
+	Vector2f pt2(point2.lat, point2.lng);
+	float angle = (loc1 - pt2).angle(pt1 - pt2);
+	if (angle == 0) {
+		// if we are exactly on the line between point1 and
+		// point2 then we are past the waypoint if the
+		// distance from location to point1 is greater then
+		// the distance from point2 to point1
+		return get_distance(&location, &point1) > 
+			get_distance(&point2, &point1);
+		
+	}
+	if (degrees(angle) > 90) {
+		return true;
+	}
+	return false;	
+}
+

libraries/AP_Math/vector2.h

@@ -142,6 +142,10 @@ struct Vector2
 	T angle(const Vector2<T> &v1, const Vector2<T> &v2)
 	{   return (T)acosf((v1*v2) / (v1.length()*v2.length()));  }
 
+	// computes the angle between this vector and another vector
+	T angle(const Vector2<T> &v2)
+		{   return (T)acosf(((*this)*v2) / (this->length()*v2.length()));  }
+
 	// computes the angle between 2 normalized arbitrary vectors
 	T angle_normalized(const Vector2<T> &v1, const Vector2<T> &v2)
 	{   return (T)acosf(v1*v2);  }

libraries/AP_Math/vector3.h

@@ -167,6 +167,10 @@ public:
 	T angle(const Vector3<T> &v1, const Vector3<T> &v2)
 	{   return (T)acosf((v1*v2) / (v1.length()*v2.length()));  }
 
+	// computes the angle between this vector and another vector
+	T angle(const Vector3<T> &v2)
+		{   return (T)acosf(((*this)*v2) / (this->length()*v2.length()));  }
+
 	// computes the angle between 2 arbitrary normalized vectors
 	T angle_normalized(const Vector3<T> &v1, const Vector3<T> &v2)
 	{   return (T)acosf(v1*v2);  }