From dd200cba314df137cb78ed87628cda5900164c9e Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Wed, 4 Jul 2012 09:19:36 +1000 Subject: [PATCH] Math: added location functions to math library these do common calculations on struct Location --- libraries/AP_Math/AP_Math.h | 18 +++- libraries/AP_Math/examples/location/Makefile | 4 + .../AP_Math/examples/location/location.pde | 101 ++++++++++++++++++ libraries/AP_Math/location.cpp | 96 +++++++++++++++++ libraries/AP_Math/vector2.h | 4 + libraries/AP_Math/vector3.h | 4 + 6 files changed, 226 insertions(+), 1 deletion(-) create mode 100644 libraries/AP_Math/examples/location/Makefile create mode 100644 libraries/AP_Math/examples/location/location.pde create mode 100644 libraries/AP_Math/location.cpp diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index dc353e57d9..0e4f546411 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/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 + diff --git a/libraries/AP_Math/examples/location/Makefile b/libraries/AP_Math/examples/location/Makefile new file mode 100644 index 0000000000..fcdc8ff8fe --- /dev/null +++ b/libraries/AP_Math/examples/location/Makefile @@ -0,0 +1,4 @@ +include ../../../AP_Common/Arduino.mk + +sitl: + make -f ../../../../libraries/Desktop/Desktop.mk diff --git a/libraries/AP_Math/examples/location/location.pde b/libraries/AP_Math/examples/location/location.pde new file mode 100644 index 0000000000..b0e08aa841 --- /dev/null +++ b/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 +#include +#include + +#ifdef DESKTOP_BUILD +// all of this is needed to build with SITL +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +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. + */ + +/* + this module deals with calculations involving struct Location + */ + +#include +#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; +} + diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h index 6a3240aa95..eebb9d8d4d 100644 --- a/libraries/AP_Math/vector2.h +++ b/libraries/AP_Math/vector2.h @@ -142,6 +142,10 @@ struct Vector2 T angle(const Vector2 &v1, const Vector2 &v2) { return (T)acosf((v1*v2) / (v1.length()*v2.length())); } + // computes the angle between this vector and another vector + T angle(const Vector2 &v2) + { return (T)acosf(((*this)*v2) / (this->length()*v2.length())); } + // computes the angle between 2 normalized arbitrary vectors T angle_normalized(const Vector2 &v1, const Vector2 &v2) { return (T)acosf(v1*v2); } diff --git a/libraries/AP_Math/vector3.h b/libraries/AP_Math/vector3.h index 71f2022e48..5759d11787 100644 --- a/libraries/AP_Math/vector3.h +++ b/libraries/AP_Math/vector3.h @@ -167,6 +167,10 @@ public: T angle(const Vector3 &v1, const Vector3 &v2) { return (T)acosf((v1*v2) / (v1.length()*v2.length())); } + // computes the angle between this vector and another vector + T angle(const Vector3 &v2) + { return (T)acosf(((*this)*v2) / (this->length()*v2.length())); } + // computes the angle between 2 arbitrary normalized vectors T angle_normalized(const Vector3 &v1, const Vector3 &v2) { return (T)acosf(v1*v2); }