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

Major formatting/whitespace cleanup

Browse Source
This commit is contained in:
px4dev
2012-10-23 23:51:13 -07:00
parent 34f99c7dca
commit 2fc1032069
50 changed files with 1112 additions and 810 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/systemlib/geo/geo.c
+69 -51
View File
@@ -89,7 +89,7 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
__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
// 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;
@@ -97,43 +97,46 @@ __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now,
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;
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) {
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.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)
__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
@@ -146,68 +149,76 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
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) {
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;
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;
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;
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 (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) {
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) {
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
// 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
// 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 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) {
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;
@@ -215,10 +226,10 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
}
}
return_var.bearing = _wrapPI(return_var.bearing);
return_var.error = false;
return return_var;
return return_var;
}
float _wrapPI(float bearing)
@@ -227,21 +238,25 @@ 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;
}
@@ -251,23 +266,26 @@ 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;
}
apps/systemlib/geo/geo.h
+7 -7
View File
@@ -44,8 +44,8 @@
* @author Lorenz Meier <lm@inf.ethz.ch>
* Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
*/
#include <stdbool.h>
typedef struct {
@@ -59,14 +59,14 @@ __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);
//
//
__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);
__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);
__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);
float _wrap180(float bearing);
float _wrap360(float bearing);
float _wrap360(float bearing);
float _wrapPI(float bearing);
float _wrap2PI(float bearing);