Moved math library to library dir, improved sensor-level HIL, cleaned up geo / conversion libs

7 files changed, 18 insertions, 700 deletions

diff --git a/src/modules/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index e01cc4dda..310fbf60f 100644
--- a/src/modules/systemlib/airspeed.c
+++ b/src/modules/systemlib/airspeed.c
@@ -42,7 +42,7 @@
 
 #include <stdio.h>
 #include <math.h>
-#include "conversions.h"
+#include <geo/geo.h>
 #include "airspeed.h"
 
 
@@ -95,17 +95,21 @@ float calc_true_airspeed_from_indicated(float speed_indicated, float pressure_am
 float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius)
 {
 	float density = get_air_density(static_pressure, temperature_celsius);
+
 	if (density < 0.0001f || !isfinite(density)) {
-	 density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
-//	 printf("[airspeed] Invalid air density, using density at sea level\n");
+		density = CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C;
 	}
 
 	float pressure_difference = total_pressure - static_pressure;
 
-	if(pressure_difference > 0) {
+	if (pressure_difference > 0) {
 		return sqrtf((2.0f*(pressure_difference)) / density);
-	} else
-	{
+	} else {
 		return -sqrtf((2.0f*fabsf(pressure_difference)) / density);
 	}
 }
+
+float get_air_density(float static_pressure, float temperature_celsius)
+{
+	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
+}
diff --git a/src/modules/systemlib/airspeed.h b/src/modules/systemlib/airspeed.h
index def53f0c1..8dccaab9c 100644
--- a/src/modules/systemlib/airspeed.h
+++ b/src/modules/systemlib/airspeed.h
@@ -85,6 +85,14 @@
   */
  __EXPORT float calc_true_airspeed(float total_pressure, float static_pressure, float temperature_celsius);
 
+ /**
+ * Calculates air density.
+ *
+ * @param static_pressure ambient pressure in millibar
+ * @param temperature_celcius air / ambient temperature in celcius
+ */
+__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
+
 __END_DECLS
 
 #endif
diff --git a/src/modules/systemlib/conversions.c b/src/modules/systemlib/conversions.c
index ac94252c5..9105d83cb 100644
--- a/src/modules/systemlib/conversions.c
+++ b/src/modules/systemlib/conversions.c
@@ -55,100 +55,3 @@ int16_t_from_bytes(uint8_t bytes[])
 
 	return u.w;
 }
-
-void rot2quat(const float R[9], float Q[4])
-{
-	float q0_2;
-	float q1_2;
-	float q2_2;
-	float q3_2;
-	int32_t idx;
-
-	/* conversion of rotation matrix to quaternion
-	 * choose the largest component to begin with */
-	q0_2 = (((1.0F + R[0]) + R[4]) + R[8]) / 4.0F;
-	q1_2 = (((1.0F + R[0]) - R[4]) - R[8]) / 4.0F;
-	q2_2 = (((1.0F - R[0]) + R[4]) - R[8]) / 4.0F;
-	q3_2 = (((1.0F - R[0]) - R[4]) + R[8]) / 4.0F;
-
-	idx = 0;
-
-	if (q0_2 < q1_2) {
-		q0_2 = q1_2;
-
-		idx = 1;
-	}
-
-	if (q0_2 < q2_2) {
-		q0_2 = q2_2;
-		idx = 2;
-	}
-
-	if (q0_2 < q3_2) {
-		q0_2 = q3_2;
-		idx = 3;
-	}
-
-	q0_2 = sqrtf(q0_2);
-
-	/* solve for the remaining three components */
-	if (idx == 0) {
-		q1_2 = q0_2;
-		q2_2 = (R[5] - R[7]) / 4.0F / q0_2;
-		q3_2 = (R[6] - R[2]) / 4.0F / q0_2;
-		q0_2 = (R[1] - R[3]) / 4.0F / q0_2;
-
-	} else if (idx == 1) {
-		q2_2 = q0_2;
-		q1_2 = (R[5] - R[7]) / 4.0F / q0_2;
-		q3_2 = (R[3] + R[1]) / 4.0F / q0_2;
-		q0_2 = (R[6] + R[2]) / 4.0F / q0_2;
-
-	} else if (idx == 2) {
-		q3_2 = q0_2;
-		q1_2 = (R[6] - R[2]) / 4.0F / q0_2;
-		q2_2 = (R[3] + R[1]) / 4.0F / q0_2;
-		q0_2 = (R[7] + R[5]) / 4.0F / q0_2;
-
-	} else {
-		q1_2 = (R[1] - R[3]) / 4.0F / q0_2;
-		q2_2 = (R[6] + R[2]) / 4.0F / q0_2;
-		q3_2 = (R[7] + R[5]) / 4.0F / q0_2;
-	}
-
-	/* return values */
-	Q[0] = q1_2;
-	Q[1] = q2_2;
-	Q[2] = q3_2;
-	Q[3] = q0_2;
-}
-
-void quat2rot(const float Q[4], float R[9])
-{
-	float q0_2;
-	float q1_2;
-	float q2_2;
-	float q3_2;
-
-	memset(&R[0], 0, 9U * sizeof(float));
-
-	q0_2 = Q[0] * Q[0];
-	q1_2 = Q[1] * Q[1];
-	q2_2 = Q[2] * Q[2];
-	q3_2 = Q[3] * Q[3];
-
-	R[0] = ((q0_2 + q1_2) - q2_2) - q3_2;
-	R[3] = 2.0F * (Q[1] * Q[2] - Q[0] * Q[3]);
-	R[6] = 2.0F * (Q[1] * Q[3] + Q[0] * Q[2]);
-	R[1] = 2.0F * (Q[1] * Q[2] + Q[0] * Q[3]);
-	R[4] = ((q0_2 + q2_2) - q1_2) - q3_2;
-	R[7] = 2.0F * (Q[2] * Q[3] - Q[0] * Q[1]);
-	R[2] = 2.0F * (Q[1] * Q[3] - Q[0] * Q[2]);
-	R[5] = 2.0F * (Q[2] * Q[3] + Q[0] * Q[1]);
-	R[8] = ((q0_2 + q3_2) - q1_2) - q2_2;
-}
-
-float get_air_density(float static_pressure, float temperature_celsius)
-{
-	return static_pressure / (CONSTANTS_AIR_GAS_CONST * (temperature_celsius - CONSTANTS_ABSOLUTE_NULL_CELSIUS));
-}
diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h
index 064426f21..dc383e770 100644
--- a/src/modules/systemlib/conversions.h
+++ b/src/modules/systemlib/conversions.h
@@ -43,7 +43,6 @@
 #define CONVERSIONS_H_
 #include <float.h>
 #include <stdint.h>
-#include <systemlib/geo/geo.h>
 
 __BEGIN_DECLS
 
@@ -57,34 +56,6 @@ __BEGIN_DECLS
  */
 __EXPORT int16_t int16_t_from_bytes(uint8_t bytes[]);
 
-/**
- * Converts a 3 x 3 rotation matrix to an unit quaternion.
- *
- * All orientations are expressed in NED frame.
- *
- * @param R rotation matrix to convert
- * @param Q quaternion to write back to
- */
-__EXPORT void rot2quat(const float R[9], float Q[4]);
-
-/**
- * Converts an unit quaternion to a 3 x 3 rotation matrix.
- *
- * All orientations are expressed in NED frame.
- *
- * @param Q quaternion to convert
- * @param R rotation matrix to write back to
- */
-__EXPORT void quat2rot(const float Q[4], float R[9]);
-
-/**
- * Calculates air density.
- *
- * @param static_pressure ambient pressure in millibar
- * @param temperature_celcius air / ambient temperature in celcius
- */
-__EXPORT float get_air_density(float static_pressure, float temperature_celsius);
-
 __END_DECLS
 
 #endif /* CONVERSIONS_H_ */
diff --git a/src/modules/systemlib/geo/geo.c b/src/modules/systemlib/geo/geo.c
deleted file mode 100644
index 6463e6489..000000000
--- a/src/modules/systemlib/geo/geo.c
+++ /dev/null
@@ -1,438 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file geo.c
- *
- * Geo / math functions to perform geodesic calculations
- *
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
- * @author Julian Oes <joes@student.ethz.ch>
- * @author Lorenz Meier <lm@inf.ethz.ch>
- */
-
-#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 */
-static double phi_1;
-static double sin_phi_1;
-static double cos_phi_1;
-static double lambda_0;
-static double scale;
-
-__EXPORT void map_projection_init(double lat_0, double lon_0) //lat_0, lon_0 are expected to be in correct format: -> 47.1234567 and not 471234567
-{
-	/* notation and formulas according to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-	phi_1 = lat_0 / 180.0 * M_PI;
-	lambda_0 = lon_0 / 180.0 * M_PI;
-
-	sin_phi_1 = sin(phi_1);
-	cos_phi_1 = cos(phi_1);
-
-	/* calculate local scale by using the relation of true distance and the distance on plane */ //TODO: this is a quick solution, there are probably easier ways to determine the scale
-
-	/* 1) calculate true distance d on sphere to a point: http://www.movable-type.co.uk/scripts/latlong.html */
-	const double r_earth = 6371000;
-
-	double lat1 = phi_1;
-	double lon1 = lambda_0;
-
-	double lat2 = phi_1 + 0.5 / 180 * M_PI;
-	double lon2 = lambda_0 + 0.5 / 180 * M_PI;
-	double sin_lat_2 = sin(lat2);
-	double cos_lat_2 = cos(lat2);
-	double d = acos(sin(lat1) * sin_lat_2 + cos(lat1) * cos_lat_2 * cos(lon2 - lon1)) * r_earth;
-
-	/* 2) calculate distance rho on plane */
-	double k_bar = 0;
-	double c =  acos(sin_phi_1 * sin_lat_2 + cos_phi_1 * cos_lat_2 * cos(lon2 - lambda_0));
-
-	if (0 != c)
-		k_bar = c / sin(c);
-
-	double x2 = k_bar * (cos_lat_2 * sin(lon2 - lambda_0)); //Projection of point 2 on plane
-	double y2 = k_bar * ((cos_phi_1 * sin_lat_2 - sin_phi_1 * cos_lat_2 * cos(lon2 - lambda_0)));
-	double rho = sqrt(pow(x2, 2) + pow(y2, 2));
-
-	scale = d / rho;
-
-}
-
-__EXPORT void map_projection_project(double lat, double lon, float *x, float *y)
-{
-	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-	double phi = lat / 180.0 * M_PI;
-	double lambda = lon / 180.0 * M_PI;
-
-	double sin_phi = sin(phi);
-	double cos_phi = cos(phi);
-
-	double k_bar = 0;
-	/* using small angle approximation (formula in comment is without aproximation) */
-	double c =  acos(sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2)); //double c =  acos( sin_phi_1 * sin_phi + cos_phi_1 * cos_phi * cos(lambda - lambda_0) );
-
-	if (0 != c)
-		k_bar = c / sin(c);
-
-	/* using small angle approximation (formula in comment is without aproximation) */
-	*y = k_bar * (cos_phi * (lambda - lambda_0)) * scale;//*y = k_bar * (cos_phi * sin(lambda - lambda_0)) * scale;
-	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * (1 - pow((lambda - lambda_0), 2) / 2))) * scale; //	*x = k_bar * ((cos_phi_1 * sin_phi - sin_phi_1 * cos_phi * cos(lambda - lambda_0))) * scale;
-
-//	printf("%phi_1=%.10f, lambda_0 =%.10f\n", phi_1, lambda_0);
-}
-
-__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon)
-{
-	/* notation and formulas accoring to: http://mathworld.wolfram.com/AzimuthalEquidistantProjection.html */
-
-	double x_descaled = x / scale;
-	double y_descaled = y / scale;
-
-	double c = sqrt(pow(x_descaled, 2) + pow(y_descaled, 2));
-	double sin_c = sin(c);
-	double cos_c = cos(c);
-
-	double lat_sphere = 0;
-
-	if (c != 0)
-		lat_sphere = asin(cos_c * sin_phi_1 + (x_descaled * sin_c * cos_phi_1) / c);
-	else
-		lat_sphere = asin(cos_c * sin_phi_1);
-
-//	printf("lat_sphere = %.10f\n",lat_sphere);
-
-	double lon_sphere = 0;
-
-	if (phi_1  == M_PI / 2) {
-		//using small angle approximation (formula in comment is without aproximation)
-		lon_sphere = (lambda_0 - y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(-y_descaled, x_descaled));
-
-	} else if (phi_1 == -M_PI / 2) {
-		//using small angle approximation (formula in comment is without aproximation)
-		lon_sphere = (lambda_0 + y_descaled / x_descaled); //lon_sphere = (lambda_0 + atan2(y_descaled, x_descaled));
-
-	} else {
-
-		lon_sphere = (lambda_0 + atan2(y_descaled * sin_c , c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c));
-		//using small angle approximation
-//    	double denominator = (c * cos_phi_1 * cos_c - x_descaled * sin_phi_1 * sin_c);
-//    	if(denominator != 0)
-//    	{
-//    		lon_sphere = (lambda_0 + (y_descaled * sin_c) / denominator);
-//    	}
-//    	else
-//    	{
-//    	...
-//    	}
-	}
-
-//	printf("lon_sphere = %.10f\n",lon_sphere);
-
-	*lat = lat_sphere * 180.0 / M_PI;
-	*lon = lon_sphere * 180.0 / M_PI;
-
-}
-
-
-__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next)
-{
-	double lat_now_rad = lat_now / 180.0d * M_PI;
-	double lon_now_rad = lon_now / 180.0d * M_PI;
-	double lat_next_rad = lat_next / 180.0d * M_PI;
-	double lon_next_rad = lon_next / 180.0d * M_PI;
-
-
-	double d_lat = lat_next_rad - lat_now_rad;
-	double d_lon = lon_next_rad - lon_now_rad;
-
-	double a = sin(d_lat / 2.0d) * sin(d_lat / 2.0d) + sin(d_lon / 2.0d) * sin(d_lon / 2.0d) * cos(lat_now_rad) * cos(lat_next_rad);
-	double c = 2.0d * atan2(sqrt(a), sqrt(1.0d - a));
-
-	const double radius_earth = 6371000.0d;
-	return radius_earth * c; 
-}
-
-__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 * 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;
-
-	/* 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));
-
-	theta = _wrap_pi(theta);
-
-	return theta;
-}
-
-// Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
-
-__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.
-
-	float dist_to_end;
-	float bearing_end;
-	float bearing_track;
-	float bearing_diff;
-
-	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_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);
-	bearing_diff = bearing_track - bearing_end;
-	bearing_diff = _wrap_pi(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) {
-		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);
-	crosstrack_error->distance = (dist_to_end) * sin(bearing_diff);
-
-	if (sin(bearing_diff) >= 0) {
-		crosstrack_error->bearing = _wrap_pi(bearing_track - M_PI_2_F);
-
-	} else {
-		crosstrack_error->bearing = _wrap_pi(bearing_track + M_PI_2_F);
-	}
-
-	return_value = OK;
-
-	return return_value;
-
-}
-
-
-__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.
-
-	// 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;
-
-	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_value;
-
-
-	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.0f) 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) {
-		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) {
-			crosstrack_error->distance = radius - dist_to_center;
-			crosstrack_error->bearing = bearing_now + M_PI_F;
-
-		} else {
-			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
-
-	} 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) {
-			crosstrack_error->distance = dist_to_start;
-			crosstrack_error->bearing = get_bearing_to_next_waypoint(lat_now, lon_now, lat_start, lon_start);
-
-		} else {
-			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);
-		}
-
-	}
-
-	crosstrack_error->bearing = _wrapPI(crosstrack_error->bearing);
-	return_value = OK;
-	return return_value;
-}
-
-__EXPORT float _wrap_pi(float bearing)
-{
-	/* value is inf or NaN */
-	if (!isfinite(bearing) || bearing == 0) {
-		return bearing;
-	}
-
-	int c = 0;
-
-	while (bearing > M_PI_F && c < 30) {
-		bearing -= M_TWOPI_F;
-		c++;
-	}
-
-	c = 0;
-
-	while (bearing <=  -M_PI_F && c < 30) {
-		bearing += M_TWOPI_F;
-		c++;
-	}
-
-	return bearing;
-}
-
-__EXPORT float _wrap_2pi(float bearing)
-{
-	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
-		return bearing;
-	}
-
-	while (bearing >= M_TWOPI_F) {
-		bearing = bearing - M_TWOPI_F;
-	}
-
-	while (bearing <  0.0f) {
-		bearing = bearing + M_TWOPI_F;
-	}
-
-	return bearing;
-}
-
-__EXPORT float _wrap_180(float bearing)
-{
-	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
-		return bearing;
-	}
-
-	while (bearing > 180.0f) {
-		bearing = bearing - 360.0f;
-	}
-
-	while (bearing <=  -180.0f) {
-		bearing = bearing + 360.0f;
-	}
-
-	return bearing;
-}
-
-__EXPORT float _wrap_360(float bearing)
-{
-	/* value is inf or NaN */
-	if (!isfinite(bearing)) {
-		return bearing;
-	}
-
-	while (bearing >= 360.0f) {
-		bearing = bearing - 360.0f;
-	}
-
-	while (bearing <  0.0f) {
-		bearing = bearing + 360.0f;
-	}
-
-	return bearing;
-}
-
-
diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h
deleted file mode 100644
index dadec51ec..000000000
--- a/src/modules/systemlib/geo/geo.h
+++ /dev/null
@@ -1,129 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
- *           Julian Oes <joes@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name PX4 nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/**
- * @file geo.h
- *
- * Definition of geo / math functions to perform geodesic calculations
- *
- * @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>
- */
-
-#pragma once
-
-__BEGIN_DECLS
-
-#include <stdbool.h>
-
-#define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
-#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C		1.225f			/* kg/m^3		*/
-#define CONSTANTS_AIR_GAS_CONST				287.1f 			/* J/(kg * K)		*/
-#define CONSTANTS_ABSOLUTE_NULL_CELSIUS			-273.15f		/* °C			*/
-#define CONSTANTS_RADIUS_OF_EARTH			6371000			/* meters (m)		*/
-
-/* compatibility aliases */
-#define RADIUS_OF_EARTH		CONSTANTS_RADIUS_OF_EARTH
-#define GRAVITY_MSS		CONSTANTS_ONE_G
-
-// XXX remove
-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
-} ;
-
-/**
- * Initializes the map transformation.
- *
- * Initializes the transformation between the geographic coordinate system and the azimuthal equidistant plane
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT void map_projection_init(double lat_0, double lon_0);
-
-/**
- * Transforms a point in the geographic coordinate system to the local azimuthal equidistant plane
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT void map_projection_project(double lat, double lon, float *x, float *y);
-
-/**
- * Transforms a point in the local azimuthal equidistant plane to the geographic coordinate system
- *
- * @param x north
- * @param y east
- * @param lat in degrees (47.1234567°, not 471234567°)
- * @param lon in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT void map_projection_reproject(float x, float y, double *lat, double *lon);
-
-/**
- * Returns the distance to the next waypoint in meters.
- *
- * @param lat_now current position in degrees (47.1234567°, not 471234567°)
- * @param lon_now current position in degrees (8.1234567°, not 81234567°)
- * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
- * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT float get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
-
-/**
- * Returns the bearing to the next waypoint in radians.
- *
- * @param lat_now current position in degrees (47.1234567°, not 471234567°)
- * @param lon_now current position in degrees (8.1234567°, not 81234567°)
- * @param lat_next next waypoint position in degrees (47.1234567°, not 471234567°)
- * @param lon_next next waypoint position in degrees (8.1234567°, not 81234567°)
- */
-__EXPORT float get_bearing_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
-
-__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 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);
-
-__EXPORT float _wrap_180(float bearing);
-__EXPORT float _wrap_360(float bearing);
-__EXPORT float _wrap_pi(float bearing);
-__EXPORT float _wrap_2pi(float bearing);
-
-__END_DECLS
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index cbf829122..94c744c03 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -45,7 +45,6 @@ SRCS		 = err.c \
 		   getopt_long.c \
 		   up_cxxinitialize.c \
 		   pid/pid.c \
-		   geo/geo.c \
 		   systemlib.c \
 		   airspeed.c \
 		   system_params.c \