Add functions for computation of the distance and bearing to the nearest point of a line segment or arc segment.

1 files changed, 23 insertions, 0 deletions

diff --git a/apps/systemlib/geo/geo.h b/apps/systemlib/geo/geo.h
index 0e86b3599..205afc79e 100644
--- a/apps/systemlib/geo/geo.h
+++ b/apps/systemlib/geo/geo.h
@@ -42,8 +42,31 @@
  * @author Thomas Gubler <thomasgubler@student.ethz.ch>
  * @author Julian Oes <joes@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
  */
+ 
+ 
+#include <stdbool.h>
+
+typedef struct {
+	bool error;			// Flag that the calculation failed
+	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 float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 
 __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);
+									 
+float _wrap180(float bearing);
+float _wrap360(float bearing);	 
+float _wrapPI(float bearing);
+float _wrap2PI(float bearing);
\ No newline at end of file