lib/geo: bugs fixed, added function add_vector_to_global_position()

2 files changed, 21 insertions, 12 deletions

diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c
index 08fe2b696..446cd5bee 100644
--- a/src/lib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -72,7 +72,6 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are
 	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
 
 	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
-	const double r_earth = 6371000;
 
 	double lat1 = phi_1;
 	double lon1 = lambda_0;
@@ -81,7 +80,7 @@ __EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are
 	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
 	double sin_lat_2 = sin(lat2);
 	double cos_lat_2 = cos(lat2);
-	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
+	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * CONSTANTS_RADIUS_OF_EARTH;
 
 	/* 2) calculate distance rho on plane */
 	double k_bar = 0;
@@ -188,8 +187,7 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
 	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 CONSTANTS_RADIUS_OF_EARTH * c;
 }
 
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
@@ -210,7 +208,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
 	return theta;
 }
 
-__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -221,11 +219,11 @@ __EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double
 	double d_lon = lon_next_rad - lon_now_rad;
 
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
-	*vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
-	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon);
+	*v_n = CONSTANTS_RADIUS_OF_EARTH * (cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon));
+	*v_e = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
 }
 
-__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e)
 {
 	double lat_now_rad = lat_now * M_DEG_TO_RAD;
 	double lon_now_rad = lon_now * M_DEG_TO_RAD;
@@ -236,8 +234,17 @@ __EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, d
 	double d_lon = lon_next_rad - lon_now_rad;
 
 	/* conscious mix of double and float trig function to maximize speed and efficiency */
-	*vy = CONSTANTS_RADIUS_OF_EARTH * d_lon;
-	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad);
+	*v_n = CONSTANTS_RADIUS_OF_EARTH * d_lat;
+	*v_e = CONSTANTS_RADIUS_OF_EARTH * d_lon * cos(lat_now_rad);
+}
+
+__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res)
+{
+	double lat_now_rad = lat_now * M_DEG_TO_RAD;
+	double lon_now_rad = lon_now * M_DEG_TO_RAD;
+
+	*lat_res = (lat_now_rad + v_n / CONSTANTS_RADIUS_OF_EARTH) * M_RAD_TO_DEG;
+	*lon_res = (lon_now_rad + v_e / (CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad))) * M_RAD_TO_DEG;
 }
 
 // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 5f4bce698..94afb4df0 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -115,9 +115,11 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou
  */
 __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
-__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
 
-__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* v_n, float* v_e);
+
+__EXPORT void add_vector_to_global_position(double lat_now, double lon_now, float v_n, float v_e, double *lat_res, double *lon_res);
 
 __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);