diff options
Diffstat (limited to 'src/lib/geo')
-rw-r--r-- | src/lib/geo/geo.c | 12 | ||||
-rw-r--r-- | src/lib/geo/geo.h | 4 |
2 files changed, 6 insertions, 10 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index e862b1dc0..43105fdba 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -210,7 +210,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub return theta; } -__EXPORT float 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* vx, float* vy) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -221,13 +221,11 @@ __EXPORT float get_vector_to_next_waypoint(double lat_now, double lon_now, doubl 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)); - - return theta; + *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); } -__EXPORT float 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* vx, float* vy) { double lat_now_rad = lat_now * M_DEG_TO_RAD; double lon_now_rad = lon_now * M_DEG_TO_RAD; @@ -240,8 +238,6 @@ __EXPORT float get_vector_to_next_waypoint_fast(double lat_now, double lon_now, /* 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); - - return theta; } // Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 0459909e4..123ff80f1 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -112,9 +112,9 @@ __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 float 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* vx, float* vy); -__EXPORT float 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* vx, float* vy); __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); |