fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-15 03:03:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Bring everything up to double

Browse Source
This commit is contained in:
Don Gagne
2014-07-06 11:06:14 -07:00
parent b9299e68d4
commit 278aafe939
1 changed files with 10 additions and 12 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/lib/geo/geo.c
+10 -12
View File
@@ -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;