diff options
author | Doug Weibel <deweibel@gmail.com> | 2012-10-21 16:39:53 -0600 |
---|---|---|
committer | Doug Weibel <deweibel@gmail.com> | 2012-10-21 16:39:53 -0600 |
commit | 1e0a34a10211dbe9894540a45dcbe428d5ae09bd (patch) | |
tree | 45223760ca728db5ce506401928b68c4610bb201 /apps/systemlib/geo/geo.c | |
parent | 64ba024db229a0e64f95a104a822670663a3763d (diff) | |
download | px4-firmware-1e0a34a10211dbe9894540a45dcbe428d5ae09bd.tar.gz px4-firmware-1e0a34a10211dbe9894540a45dcbe428d5ae09bd.tar.bz2 px4-firmware-1e0a34a10211dbe9894540a45dcbe428d5ae09bd.zip |
Add functions for computation of the distance and bearing to the nearest point of a line segment or arc segment.
Diffstat (limited to 'apps/systemlib/geo/geo.c')
-rw-r--r-- | apps/systemlib/geo/geo.c | 202 |
1 files changed, 192 insertions, 10 deletions
diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c index ce46d01cc..bc467bfa3 100644 --- a/apps/systemlib/geo/geo.c +++ b/apps/systemlib/geo/geo.c @@ -68,10 +68,10 @@ __EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, dou __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next) { - double lat_now_rad = lat_now / 180.0 * M_PI; - double lon_now_rad = lon_now / 180.0 * M_PI; - double lat_next_rad = lat_next / 180.0 * M_PI; - double lon_next_rad = lon_next / 180.0 * M_PI; + double lat_now_rad = lat_now * M_DEG_TO_RAD; + double lon_now_rad = lon_now * M_DEG_TO_RAD; + double lat_next_rad = lat_next * M_DEG_TO_RAD; + double lon_next_rad = lon_next * M_DEG_TO_RAD; double d_lat = lat_next_rad - lat_now_rad; double d_lon = lon_next_rad - lon_now_rad; @@ -79,13 +79,195 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub /* conscious mix of double and float trig function to maximize speed and efficiency */ float theta = atan2f(sin(d_lon) * cos(lat_next_rad) , cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon)); - if (theta < M_PI_F) { - theta += 2.0f * M_PI_F; + theta = _wrapPI(theta); + + return theta; +} + +// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> + +__EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now, double lat_start, double lon_start, double lat_end, double lon_end) +{ +// This function returns the distance to the nearest point on the track line. Distance is positive if current +// position is right of the track and negative if left of the track as seen from a point on the track line +// headed towards the end point. + + crosstrack_error_s return_var; + float dist_to_end; + float bearing_end; + float bearing_track; + float bearing_diff; + + return_var.error = true; // Set error flag, cleared when valid result calculated. + return_var.past_end = false; + return_var.distance = 0.0f; + return_var.bearing = 0.0f; + + // Return error if arguments are bad + if(lat_now == 0.0d || lon_now == 0.0d || lat_start == 0.0d || lon_start == 0.0d || lat_end == 0.0d || lon_end == 0.0d) return return_var; + + bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + bearing_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end); + bearing_diff = bearing_track - bearing_end; + bearing_diff = _wrapPI(bearing_diff); + + // Return past_end = true if past end point of line + if(bearing_diff > M_PI_2_F || bearing_diff < -M_PI_2_F) { + return_var.past_end = true; + return_var.error = false; + return return_var; } + + dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + return_var.distance = (dist_to_end)*sin(bearing_diff); + if(sin(bearing_diff) >=0 ) { + return_var.bearing = _wrapPI(bearing_track - M_PI_2_F); + } else { + return_var.bearing = _wrapPI(bearing_track + M_PI_2_F); + } + return_var.error = false; + + return return_var; + +} + + +__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, + float radius, float arc_start_bearing, float arc_sweep) +{ + // This function returns the distance to the nearest point on the track arc. Distance is positive if current + // position is right of the arc and negative if left of the arc as seen from the closest point on the arc and + // headed towards the end point. + crosstrack_error_s return_var; - if (theta > M_PI_F) { - theta -= 2.0f * M_PI_F; + // Determine if the current position is inside or outside the sector between the line from the center + // to the arc start and the line from the center to the arc end + float bearing_sector_start; + float bearing_sector_end; + float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + bool in_sector; + + return_var.error = true; // Set error flag, cleared when valid result calculated. + return_var.past_end = false; + return_var.distance = 0.0f; + return_var.bearing = 0.0f; + + // Return error if arguments are bad + if(lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_var; + + + if(arc_sweep >= 0) { + bearing_sector_start = arc_start_bearing; + bearing_sector_end = arc_start_bearing + arc_sweep; + if(bearing_sector_end > 2.0f*M_PI_F) bearing_sector_end -= M_TWOPI_F; + } else { + bearing_sector_end = arc_start_bearing; + bearing_sector_start = arc_start_bearing - arc_sweep; + if(bearing_sector_start < 0.0) bearing_sector_start += M_TWOPI_F; } + in_sector = false; + + // Case where sector does not span zero + if(bearing_sector_end >= bearing_sector_start && bearing_now >= bearing_sector_start && bearing_now <= bearing_sector_end) in_sector = true; + + // Case where sector does span zero + if(bearing_sector_end < bearing_sector_start && ( bearing_now > bearing_sector_start || bearing_now < bearing_sector_end ) ) in_sector = true; + + // If in the sector then calculate distance and bearing to closest point + if(in_sector) { + return_var.past_end = false; + float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); + + if(dist_to_center <= radius) { + return_var.distance = radius - dist_to_center; + return_var.bearing = bearing_now + M_PI_F; + } else { + return_var.distance = dist_to_center - radius; + return_var.bearing = bearing_now; + } + + // If out of the sector then calculate dist and bearing to start or end point + } else { + + // Use the approximation that 111,111 meters in the y direction is 1 degree (of latitude) + // and 111,111 * cos(latitude) meters in the x direction is 1 degree (of longitude) to + // 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 * sin(arc_start_bearing); + float start_disp_y = radius * cos(arc_start_bearing); + float end_disp_x = radius * sin(_wrapPI(arc_start_bearing+arc_sweep)); + float end_disp_y = radius * cos(_wrapPI(arc_start_bearing+arc_sweep)); + float lon_start = lon_now + start_disp_x/111111.0d; + float lat_start = lat_now + start_disp_y*cos(lat_now)/111111.0d; + float lon_end = lon_now + end_disp_x/111111.0d; + float lat_end = lat_now + end_disp_y*cos(lat_now)/111111.0d; + 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); + if(dist_to_start < dist_to_end) { + return_var.distance = dist_to_start; + return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); + } else { + return_var.past_end = true; + return_var.distance = dist_to_end; + return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); + } - return theta; -}
\ No newline at end of file + } + + return_var.bearing = _wrapPI(return_var.bearing); + return_var.error = false; + return return_var; +} + +float _wrapPI(float bearing) +{ + + while (bearing > M_PI_F) { + bearing = bearing - M_TWOPI_F; + } + while (bearing <= -M_PI_F) { + bearing = bearing + M_TWOPI_F; + } + return bearing; +} + +float _wrap2PI(float bearing) +{ + + while (bearing >= M_TWOPI_F) { + bearing = bearing - M_TWOPI_F; + } + while (bearing < 0.0f) { + bearing = bearing + M_TWOPI_F; + } + return bearing; +} + +float _wrap180(float bearing) +{ + + while (bearing > 180.0f) { + bearing = bearing - 360.0f; + } + while (bearing <= -180.0f) { + bearing = bearing + 360.0f; + } + return bearing; +} + +float _wrap360(float bearing) +{ + + while (bearing >= 360.0f) { + bearing = bearing - 360.0f; + } + while (bearing < 0.0f) { + bearing = bearing + 360.0f; + } + return bearing; +} + + +
\ No newline at end of file |