@ -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 ;