Browse Source

Bring everything up to double

sbg
Don Gagne 11 years ago
parent
commit
278aafe939
  1. 22
      src/lib/geo/geo.c

22
src/lib/geo/geo.c

@ -294,23 +294,21 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do @@ -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;

Loading…
Cancel
Save