diff options
Diffstat (limited to 'src/modules/systemlib/geo')
-rw-r--r-- | src/modules/systemlib/geo/geo.c | 5 | ||||
-rw-r--r-- | src/modules/systemlib/geo/geo.h | 16 |
2 files changed, 18 insertions, 3 deletions
diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c index 6746e8ff3..6463e6489 100644 --- a/src/modules/systemlib/geo/geo.c +++ b/src/modules/systemlib/geo/geo.c @@ -185,12 +185,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; - double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); + double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad); double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); const double radius_earth = 6371000.0d; - - return radius_earth * c; + return radius_earth * c; } __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h index 0c0b5c533..84097b49f 100644 --- a/src/modules/systemlib/geo/geo.h +++ b/src/modules/systemlib/geo/geo.h @@ -82,8 +82,24 @@ __EXPORT void map_projection_project(double lat, double lon, float *x, float *y) */ __EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon); +/** + * Returns the distance to the next waypoint in meters. + * + * @param lat_now current position in degrees (47.1234567°, not 471234567°) + * @param lon_now current position in degrees (8.1234567°, not 81234567°) + * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) + * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) + */ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); +/** + * Returns the bearing to the next waypoint in radians. + * + * @param lat_now current position in degrees (47.1234567°, not 471234567°) + * @param lon_now current position in degrees (8.1234567°, not 81234567°) + * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°) + * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°) + */ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end); |