From f435025d26f49d1d9069282aa72c7e1cb9201773 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 4 Jun 2013 00:10:58 +0200 Subject: Completed main implementation and debugging --- src/modules/systemlib/geo/geo.c | 8 ++++---- src/modules/systemlib/geo/geo.h | 16 ++++++++++++++++ 2 files changed, 20 insertions(+), 4 deletions(-) (limited to 'src/modules/systemlib') diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c index 6746e8ff3..3ac4bd168 100644 --- a/src/modules/systemlib/geo/geo.c +++ b/src/modules/systemlib/geo/geo.c @@ -46,7 +46,7 @@ #include #include -#include +//#include #include #include #include @@ -185,12 +185,12 @@ __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; + printf("DIST: %.4f\n", 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); -- cgit v1.2.3 From 82c7e58122992aab2cf698951f9a33817cf1a050 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Tue, 4 Jun 2013 01:03:16 +0200 Subject: Removed some debugging code --- src/modules/systemlib/geo/geo.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) (limited to 'src/modules/systemlib') diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c index 3ac4bd168..6463e6489 100644 --- a/src/modules/systemlib/geo/geo.c +++ b/src/modules/systemlib/geo/geo.c @@ -46,7 +46,7 @@ #include #include -//#include +#include #include #include #include @@ -189,7 +189,6 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a)); const double radius_earth = 6371000.0d; - printf("DIST: %.4f\n", radius_earth * c); return radius_earth * c; } -- cgit v1.2.3