aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_Math: add overloaded functions get_horizontal_distance_cm() and get_bearing_cd() (NFC)

Browse Source
This commit is contained in:
Dr.-Ing. Amilcar Do Carmo Lucas 2017-12-03 22:04:30 +01:00 committed by Randy Mackay
parent 0404d3588d
commit e13281ab2d
2 changed files with 23 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
libraries/AP_Math/location.cpp
View File

@ -46,6 +46,12 @@ uint32_t get_distance_cm(const struct Location &loc1, const struct Location &loc
return get_distance(loc1, loc2) * 100;
}
// return horizontal distance between two positions in cm
float get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination)
{
return norm(destination.x-origin.x,destination.y-origin.y);
}
// return bearing in centi-degrees between two locations
int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2)
{
@ -56,6 +62,16 @@ int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2)
return bearing;
}
// return bearing in centi-degrees between two positions
float get_bearing_cd(const Vector3f &origin, const Vector3f &destination)
{
float bearing = atan2f(destination.y-origin.y, destination.x-origin.x) * DEGX100;
if (bearing < 0) {
bearing += 36000.0f;
}
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

8
libraries/AP_Math/location.h
View File

@ -8,7 +8,7 @@
#include "vector2.h"
#include "vector3.h"
// scaling factor from 1e-7 degrees to meters at equater
// scaling factor from 1e-7 degrees to meters at equator
// == 1.0e-7 * DEG_TO_RAD * RADIUS_OF_EARTH
#define LOCATION_SCALING_FACTOR 0.011131884502145034f
// inverse of LOCATION_SCALING_FACTOR
@ -27,9 +27,15 @@ float get_distance(const struct Location &loc1, const struct Location &lo
// return distance in centimeters between two locations
uint32_t get_distance_cm(const struct Location &loc1, const struct Location &loc2);
// return horizontal distance in centimeters between two positions
float get_horizontal_distance_cm(const Vector3f &origin, const Vector3f &destination);
// return bearing in centi-degrees between two locations
int32_t get_bearing_cd(const struct Location &loc1, const struct Location &loc2);
// return bearing in centi-degrees between two positions
float get_bearing_cd(const Vector3f &origin, const Vector3f &destination);
// 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