From 278aafe939bc20c2882d17b6e21da5ab38d1f400 Mon Sep 17 00:00:00 2001 From: Don Gagne Date: Sun, 6 Jul 2014 11:06:14 -0700 Subject: Bring everything up to double --- src/lib/geo/geo.c | 22 ++++++++++------------ 1 file changed, 10 insertions(+), 12 deletions(-) (limited to 'src') diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 230be9281..e600976ce 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -294,23 +294,21 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do // calculate the position of the start and end points. We should not be doing this often // as this function generally will not be called repeatedly when we are out of the sector. - // TO DO - this is messed up and won't compile - float start_disp_x = radius * sinf(arc_start_bearing); - float start_disp_y = radius * cosf(arc_start_bearing); - float end_disp_x = radius * sinf(_wrapPI((double)(arc_start_bearing + arc_sweep))); - float end_disp_y = radius * cosf(_wrapPI((double)(arc_start_bearing + arc_sweep))); - float lon_start = (float)lon_now + start_disp_x / 111111.0f; - float lat_start = (float)lat_now + start_disp_y * cosf(lat_now) / 111111.0f; - float lon_end = (float)lon_now + end_disp_x / 111111.0f; - float lat_end = (float)lat_now + end_disp_y * cosf(lat_now) / 111111.0f; - float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); - float dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + double start_disp_x = (double)radius * sin(arc_start_bearing); + double start_disp_y = (double)radius * cos(arc_start_bearing); + double end_disp_x = (double)radius * sin(_wrapPI((double)(arc_start_bearing + arc_sweep))); + double end_disp_y = (double)radius * cos(_wrapPI((double)(arc_start_bearing + arc_sweep))); + double lon_start = lon_now + start_disp_x / 111111.0; + double lat_start = lat_now + start_disp_y * cos(lat_now) / 111111.0; + double lon_end = lon_now + end_disp_x / 111111.0; + double lat_end = lat_now + end_disp_y * cos(lat_now) / 111111.0; + double dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + double dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); if (dist_to_start < dist_to_end) { crosstrack_error->distance = dist_to_start; crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); - } else { crosstrack_error->past_end = true; crosstrack_error->distance = dist_to_end; -- cgit v1.2.3