diff options
| author | Don Gagne <don@thegagnes.com> | 2014-07-06 11:06:14 -0700 |
|---|---|---|
| committer | Don Gagne <don@thegagnes.com> | 2014-07-06 11:06:14 -0700 |
| commit | 278aafe939bc20c2882d17b6e21da5ab38d1f400 (patch) | |
| tree | c07eb8bae5eac32ec8a6b0f3e57821d2cf50d2ac | |
| parent | b9299e68d4147845bab9ed99509b3e50b7c94ae1 (diff) | |
| download | px4-firmware-278aafe939bc20c2882d17b6e21da5ab38d1f400.tar.gz px4-firmware-278aafe939bc20c2882d17b6e21da5ab38d1f400.tar.bz2 px4-firmware-278aafe939bc20c2882d17b6e21da5ab38d1f400.zip | |
Bring everything up to double
| -rw-r--r-- | src/lib/geo/geo.c | 22 |
1 files changed, 10 insertions, 12 deletions
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; |