Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-09-28 01:18:55 +08:00
Code Issues Packages Projects Releases Wiki Activity

small code cleanup

Browse Source
This commit is contained in:
Thomas Gubler
2012-11-11 17:35:55 +01:00
parent 8edf02681b
commit 60198e3a2d
3 changed files with 48 additions and 45 deletions
Download Patch File Download Diff File Expand all files Collapse all files
apps/fixedwing_pos_control/fixedwing_pos_control_main.c
+4 -4
View File
@@ -169,7 +169,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
memset(&global_setpoint, 0, sizeof(global_setpoint)); memset(&global_setpoint, 0, sizeof(global_setpoint));
struct vehicle_attitude_s att; struct vehicle_attitude_s att;
memset(&att, 0, sizeof(att)); memset(&att, 0, sizeof(att));
crosstrack_error_s xtrack_err; struct crosstrack_error_s xtrack_err;
memset(&xtrack_err, 0, sizeof(xtrack_err)); memset(&xtrack_err, 0, sizeof(xtrack_err));
/* output structs */ /* output structs */
@@ -257,11 +257,11 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
/* calculate crosstrack error */ /* calculate crosstrack error */
// Only the case of a straight line track following handled so far // Only the case of a straight line track following handled so far
xtrack_err = get_distance_to_line((double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d, int distance_res = get_distance_to_line(&xtrack_err, (double)global_pos.lat / (double)1e7d, (double)global_pos.lon / (double)1e7d,
(double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d, (double)start_pos.lat / (double)1e7d, (double)start_pos.lon / (double)1e7d,
(double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d); (double)global_setpoint.lat / (double)1e7d, (double)global_setpoint.lon / (double)1e7d);
if(!(xtrack_err.error || xtrack_err.past_end)) { if(!(distance_res != OK || xtrack_err.past_end)) {
float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards float delta_psi_c = -p.xtrack_p * xtrack_err.distance; //(-) because z axis points downwards
@@ -286,7 +286,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
} }
else { else {
if (counter % 100 == 0) if (counter % 100 == 0)
printf("error: %d, past_end %d\n", xtrack_err.error, xtrack_err.past_end); printf("distance_res: %d, past_end %d\n", distance_res, xtrack_err.past_end);
} }
} }
apps/systemlib/geo/geo.c
+40 -36
View File
@@ -45,7 +45,12 @@
*/ */
#include <systemlib/geo/geo.h> #include <systemlib/geo/geo.h>
#include <nuttx/config.h>
#include <unistd.h>
#include <pthread.h>
#include <stdio.h>
#include <math.h> #include <math.h>
#include <stdbool.h>
/* values for map projection */ /* values for map projection */
@@ -230,25 +235,24 @@ __EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, doub
// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu> // 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) __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, 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 // 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. // headed towards the end point.
crosstrack_error_s return_var;
float dist_to_end; float dist_to_end;
float bearing_end; float bearing_end;
float bearing_track; float bearing_track;
float bearing_diff; float bearing_diff;
return_var.error = true; // Set error flag, cleared when valid result calculated. int return_value = ERROR; // Set error flag, cleared when valid result calculated.
return_var.past_end = false; crosstrack_error->past_end = false;
return_var.distance = 0.0f; crosstrack_error->distance = 0.0f;
return_var.bearing = 0.0f; crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad // 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_value;
bearing_end = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); 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_track = get_bearing_to_next_waypoint(lat_start, lon_start, lat_end, lon_end);
@@ -257,35 +261,34 @@ __EXPORT crosstrack_error_s get_distance_to_line(double lat_now, double lon_now,
// Return past_end = true if past end point of line // 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; crosstrack_error->past_end = true;
return_var.error = false; return_value = OK;
return return_var; return return_value;
} }
dist_to_end = get_distance_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); 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); crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
if (sin(bearing_diff) >= 0) { if (sin(bearing_diff) >= 0) {
return_var.bearing = _wrapPI(bearing_track - M_PI_2_F); crosstrack_error->bearing = _wrapPI(bearing_track - M_PI_2_F);
} else { } else {
return_var.bearing = _wrapPI(bearing_track + M_PI_2_F); crosstrack_error->bearing = _wrapPI(bearing_track + M_PI_2_F);
} }
return_var.error = false; return_value = OK;
return return_var; return return_value;
} }
__EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now, double lat_center, double lon_center, __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep) 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 // 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 // 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. // headed towards the end point.
crosstrack_error_s return_var;
// Determine if the current position is inside or outside the sector between the line from the center // 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 // to the arc start and the line from the center to the arc end
@@ -294,13 +297,13 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); float bearing_now = get_bearing_to_next_waypoint(lat_now, lon_now, lat_center, lon_center);
bool in_sector; bool in_sector;
return_var.error = true; // Set error flag, cleared when valid result calculated. int return_value = ERROR; // Set error flag, cleared when valid result calculated.
return_var.past_end = false; crosstrack_error->past_end = false;
return_var.distance = 0.0f; crosstrack_error->distance = 0.0f;
return_var.bearing = 0.0f; crosstrack_error->bearing = 0.0f;
// Return error if arguments are bad // 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 (lat_now == 0.0d || lon_now == 0.0d || lat_center == 0.0d || lon_center == 0.0d || radius == 0.0d) return return_value;
if (arc_sweep >= 0) { if (arc_sweep >= 0) {
@@ -313,7 +316,7 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
bearing_sector_end = arc_start_bearing; bearing_sector_end = arc_start_bearing;
bearing_sector_start = arc_start_bearing - arc_sweep; 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.0f) bearing_sector_start += M_TWOPI_F;
} }
in_sector = false; in_sector = false;
@@ -326,16 +329,16 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
// If in the sector then calculate distance and bearing to closest point // If in the sector then calculate distance and bearing to closest point
if (in_sector) { if (in_sector) {
return_var.past_end = false; crosstrack_error->past_end = false;
float dist_to_center = get_distance_to_next_waypoint(lat_now, lon_now, lat_center, lon_center); 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; crosstrack_error->distance = radius - dist_to_center;
return_var.bearing = bearing_now + M_PI_F; crosstrack_error->bearing = bearing_now + M_PI_F;
} else { } else {
return_var.distance = dist_to_center - radius; crosstrack_error->distance = dist_to_center - radius;
return_var.bearing = bearing_now; crosstrack_error->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
@@ -359,21 +362,22 @@ __EXPORT crosstrack_error_s get_distance_to_arc(double lat_now, double lon_now,
float dist_to_start = get_distance_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); 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); 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; crosstrack_error->distance = dist_to_start;
return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start); crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
} else { } else {
return_var.past_end = true; crosstrack_error->past_end = true;
return_var.distance = dist_to_end; crosstrack_error->distance = dist_to_end;
return_var.bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end); crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
} }
} }
return_var.bearing = _wrapPI(return_var.bearing); crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing);
return_var.error = false; return_value = OK;
return return_var; return return_value;
} }
float _wrapPI(float bearing) float _wrapPI(float bearing)
apps/systemlib/geo/geo.h
+4 -5
View File
@@ -48,12 +48,11 @@
#include <stdbool.h> #include <stdbool.h>
typedef struct { struct crosstrack_error_s {
bool error; // Flag that the calculation failed
bool past_end; // Flag indicating we are past the end of the line/arc segment bool past_end; // Flag indicating we are past the end of the line/arc segment
float distance; // Distance in meters to closest point on line/arc float distance; // Distance in meters to closest point on line/arc
float bearing; // Bearing in radians to closest point on line/arc float bearing; // Bearing in radians to closest point on line/arc
} crosstrack_error_s; } ;
__EXPORT static void map_projection_init(double lat_0, double lon_0); __EXPORT static void map_projection_init(double lat_0, double lon_0);
@@ -67,9 +66,9 @@ __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); __EXPORT int get_distance_to_line(struct crosstrack_error_s * crosstrack_error, 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, __EXPORT int get_distance_to_arc(struct crosstrack_error_s * crosstrack_error, double lat_now, double lon_now, double lat_center, double lon_center,
float radius, float arc_start_bearing, float arc_sweep); float radius, float arc_start_bearing, float arc_sweep);
float _wrap180(float bearing); float _wrap180(float bearing);