From 672acdc8ef503cc1046b66be70b1b1a222bb25ec Mon Sep 17 00:00:00 2001 From: dgrat Date: Thu, 25 Feb 2016 13:07:27 +0100 Subject: [PATCH] AP_Math: Created location.h header for location functions Helps to order AP_Math functions by purpose. --- libraries/AP_Math/AP_Math.h | 63 +--------------------------- libraries/AP_Math/location.cpp | 1 + libraries/AP_Math/location.h | 76 ++++++++++++++++++++++++++++++++++ 3 files changed, 78 insertions(+), 62 deletions(-) create mode 100644 libraries/AP_Math/location.h diff --git a/libraries/AP_Math/AP_Math.h b/libraries/AP_Math/AP_Math.h index 3e8d056f3c..2c8fa9c9e5 100644 --- a/libraries/AP_Math/AP_Math.h +++ b/libraries/AP_Math/AP_Math.h @@ -18,6 +18,7 @@ #include "polygon.h" #include "edc.h" #include +#include "location.h" // define AP_Param types AP_Vector3f and Ap_Matrix3f @@ -35,19 +36,6 @@ float safe_asin(float v); // a varient of sqrt() that always gives a valid answer. float safe_sqrt(float v); -// longitude_scale - returns the scaler to compensate for shrinking longitude as you move north or south from the equator -// Note: this does not include the scaling to convert longitude/latitude points to meters or centimeters -float longitude_scale(const struct Location &loc); - -// return distance in meters between two locations -float get_distance(const struct Location &loc1, const struct Location &loc2); - -// return distance in centimeters between two locations -uint32_t get_distance_cm(const struct Location &loc1, const struct Location &loc2); - -// return bearing in centi-degrees between two locations -int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2); - // return determinant of square matrix float detnxn(const float C[], const uint8_t n); @@ -63,35 +51,6 @@ bool inverse4x4(float m[],float invOut[]); // matrix multiplication of two NxN matrices float* mat_mul(float *A, float *B, uint8_t n); -// 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(const struct Location & location, - const struct Location & point1, - const struct Location & point2); - -/* - return the proportion we are along the path from point1 to - point2. This will be less than >1 if we have passed point2 - */ -float location_path_proportion(const struct Location &location, - const struct Location &point1, - const 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); - -/* - return the distance in meters in North/East plane as a N/E vector - from loc1 to loc2 - */ -Vector2f location_diff(const struct Location &loc1, const struct Location &loc2); - /* wrap an angle in centi-degrees */ @@ -110,26 +69,6 @@ float wrap_PI(float angle_in_radians); */ float wrap_2PI(float angle); -/* - * check if lat and lng match. Ignore altitude and options - */ -bool locations_are_same(const struct Location &loc1, const struct Location &loc2); - -/* - print a int32_t lat/long in decimal degrees - */ -void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon); - -// Converts from WGS84 geodetic coordinates (lat, lon, height) -// into WGS84 Earth Centered, Earth Fixed (ECEF) coordinates -// (X, Y, Z) -void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef); - -// Converts from WGS84 Earth Centered, Earth Fixed (ECEF) -// coordinates (X, Y, Z), into WHS84 geodetic -// coordinates (lat, lon, height) -void wgsecef2llh(const Vector3d &ecef, Vector3d &llh); - // constrain a value // constrain a value static inline float constrain_float(float amt, float low, float high) diff --git a/libraries/AP_Math/location.cpp b/libraries/AP_Math/location.cpp index 2a18103999..28c7f30331 100644 --- a/libraries/AP_Math/location.cpp +++ b/libraries/AP_Math/location.cpp @@ -23,6 +23,7 @@ #include #include #include "AP_Math.h" +#include "location.h" // scaling factor from 1e-7 degrees to meters at equater // == 1.0e-7 * DEG_TO_RAD * RADIUS_OF_EARTH diff --git a/libraries/AP_Math/location.h b/libraries/AP_Math/location.h new file mode 100644 index 0000000000..a696df9433 --- /dev/null +++ b/libraries/AP_Math/location.h @@ -0,0 +1,76 @@ +#pragma once + +#include + +#include +#include + +#include "vector2.h" +#include "vector3.h" + + +/* + * LOCATION + */ +// longitude_scale - returns the scaler to compensate for shrinking longitude as you move north or south from the equator +// Note: this does not include the scaling to convert longitude/latitude points to meters or centimeters +float longitude_scale(const struct Location &loc); + +// return distance in meters between two locations +float get_distance(const struct Location &loc1, const struct Location &loc2); + +// return distance in centimeters between two locations +uint32_t get_distance_cm(const struct Location &loc1, const struct Location &loc2); + +// return bearing in centi-degrees between two locations +int32_t get_bearing_cd(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(const struct Location & location, + const struct Location & point1, + const struct Location & point2); + +/* + return the proportion we are along the path from point1 to + point2. This will be less than >1 if we have passed point2 + */ +float location_path_proportion(const struct Location &location, + const struct Location &point1, + const 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); + +/* + return the distance in meters in North/East plane as a N/E vector + from loc1 to loc2 + */ +Vector2f location_diff(const struct Location &loc1, const struct Location &loc2); + +/* + * check if lat and lng match. Ignore altitude and options + */ +bool locations_are_same(const struct Location &loc1, const struct Location &loc2); + +/* + print a int32_t lat/long in decimal degrees + */ +void print_latlon(AP_HAL::BetterStream *s, int32_t lat_or_lon); + +// Converts from WGS84 geodetic coordinates (lat, lon, height) +// into WGS84 Earth Centered, Earth Fixed (ECEF) coordinates +// (X, Y, Z) +void wgsllh2ecef(const Vector3d &llh, Vector3d &ecef); + +// Converts from WGS84 Earth Centered, Earth Fixed (ECEF) +// coordinates (X, Y, Z), into WHS84 geodetic +// coordinates (lat, lon, height) +void wgsecef2llh(const Vector3d &ecef, Vector3d &llh); +