diff options
| author | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-11 13:37:08 +0200 |
|---|---|---|
| committer | Thomas Gubler <thomasgubler@gmail.com> | 2014-08-11 13:37:08 +0200 |
| commit | af1ad04c2390f69d9f11394f872a0086206d044e (patch) | |
| tree | 0a392555bc6d1a5098b3d4a89587463427ccf64d /src/lib/geo | |
| parent | 801d1d31983d404d5ebe8f5750359f2d8c7fdf43 (diff) | |
| parent | 5abdacc9079e4ebe5f2e3d855f3c5241adecef37 (diff) | |
| download | px4-firmware-af1ad04c2390f69d9f11394f872a0086206d044e.tar.gz px4-firmware-af1ad04c2390f69d9f11394f872a0086206d044e.tar.bz2 px4-firmware-af1ad04c2390f69d9f11394f872a0086206d044e.zip | |
Merge remote-tracking branch 'origin/master' into geo
Diffstat (limited to 'src/lib/geo')
| -rw-r--r-- | src/lib/geo/geo.c | 24 |
1 files changed, 11 insertions, 13 deletions
diff --git a/src/lib/geo/geo.c b/src/lib/geo/geo.c index 36be32158..ad1ff37e3 100644 --- a/src/lib/geo/geo.c +++ b/src/lib/geo/geo.c @@ -460,23 +460,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(arc_start_bearing + arc_sweep)); - float end_disp_y = radius * cosf(_wrapPI(arc_start_bearing + arc_sweep)); - float lon_start = lon_now + start_disp_x / 111111.0f; - float lat_start = lat_now + start_disp_y * cosf(lat_now) / 111111.0f; - float lon_end = lon_now + end_disp_x / 111111.0f; - float lat_end = 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; @@ -485,7 +483,7 @@ __EXPORT int get_distance_to_arc(struct crosstrack_error_s *crosstrack_error, do } - crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing); + crosstrack_error->bearing = _wrapPI((double)crosstrack_error->bearing); return_value = OK; return return_value; } |