aboutsummaryrefslogtreecommitdiff
path: root/apps/systemlib
diff options
context:
space:
mode:
authorLorenz Meier <lm@inf.ethz.ch>2012-11-13 18:57:35 +0100
committerLorenz Meier <lm@inf.ethz.ch>2012-11-13 18:57:35 +0100
commita6294be6f076913d7b2c04e42aae1c0c93193a6f (patch)
treeaab14c218bbd54cab50a4706c661d612a964476e /apps/systemlib
parentd15e6ae73a57da9e16e61b58576c7a5bfddf6517 (diff)
parent403874d313464c72da309483e6e8271e9c70f965 (diff)
downloadpx4-firmware-a6294be6f076913d7b2c04e42aae1c0c93193a6f.tar.gz
px4-firmware-a6294be6f076913d7b2c04e42aae1c0c93193a6f.tar.bz2
px4-firmware-a6294be6f076913d7b2c04e42aae1c0c93193a6f.zip
Merge branch 'fw_control' of https://github.com/thomasgubler/Firmware
Diffstat (limited to 'apps/systemlib')
-rw-r--r--apps/systemlib/geo/geo.c76
-rw-r--r--apps/systemlib/geo/geo.h9
2 files changed, 44 insertions, 41 deletions
diff --git a/apps/systemlib/geo/geo.c b/apps/systemlib/geo/geo.c
index abe44c69e..2f4b37e79 100644
--- a/apps/systemlib/geo/geo.c
+++ b/apps/systemlib/geo/geo.c
@@ -45,7 +45,12 @@
*/
#include <systemlib/geo/geo.h>
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <pthread.h>
+#include <stdio.h>
#include <math.h>
+#include <stdbool.h>
/* 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>
-__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
// 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;
+ int return_value = ERROR; // Set error flag, cleared when valid result calculated.
+ crosstrack_error->past_end = false;
+ crosstrack_error->distance = 0.0f;
+ crosstrack_error->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_value;
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);
@@ -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
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;
+ crosstrack_error->past_end = true;
+ return_value = OK;
+ return return_value;
}
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) {
- return_var.bearing = _wrapPI(bearing_track - M_PI_2_F);
+ crosstrack_error->bearing = _wrapPI(bearing_track - M_PI_2_F);
} 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)
{
// 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;
// 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
@@ -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);
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;
+ int return_value = ERROR; // Set error flag, cleared when valid result calculated.
+ crosstrack_error->past_end = false;
+ crosstrack_error->distance = 0.0f;
+ crosstrack_error->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 (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) {
@@ -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_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;
@@ -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_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);
if (dist_to_center <= radius) {
- return_var.distance = radius - dist_to_center;
- return_var.bearing = bearing_now + M_PI_F;
+ crosstrack_error->distance = radius - dist_to_center;
+ crosstrack_error->bearing = bearing_now + M_PI_F;
} else {
- return_var.distance = dist_to_center - radius;
- return_var.bearing = bearing_now;
+ crosstrack_error->distance = dist_to_center - radius;
+ crosstrack_error->bearing = bearing_now;
}
// 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_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);
+ crosstrack_error->distance = dist_to_start;
+ crosstrack_error->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);
+ crosstrack_error->past_end = true;
+ crosstrack_error->distance = dist_to_end;
+ crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_end, lon_end);
}
}
- return_var.bearing = _wrapPI(return_var.bearing);
- return_var.error = false;
- return return_var;
+ crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing);
+ return_value = OK;
+ return return_value;
}
float _wrapPI(float bearing)
diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h
index 807e508a2..7aad79a8c 100644
--- a/apps/systemlib/geo/geo.h
+++ b/apps/systemlib/geo/geo.h
@@ -48,12 +48,11 @@
#include <stdbool.h>
-typedef struct {
- bool error; // Flag that the calculation failed
+struct crosstrack_error_s {
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 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);
@@ -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 _wrap180(float bearing);