69 files changed, 216 insertions, 171 deletions

diff --git a/ROMFS/px4fmu_common/init.d/rc.hil b/ROMFS/px4fmu_common/init.d/rc.hil
index 3517a5bd8..a843b7ffb 100644
--- a/ROMFS/px4fmu_common/init.d/rc.hil
+++ b/ROMFS/px4fmu_common/init.d/rc.hil
@@ -8,7 +8,7 @@ echo "[HIL] starting.."
 uorb start
 
 # Tell MAVLink that this link is "fast"
-mavlink start -b 230400 -d /dev/console
+mavlink start -b 230400 -d /dev/ttyS1
 
 # Create a fake HIL /dev/pwm_output interface
 hil mode_pwm
@@ -38,13 +38,13 @@ commander start
 #
 # Check if we got an IO
 #
-if [ px4io start ]
+if px4io start
 then
 	echo "IO started"
 else
 	fmu mode_serial
 	echo "FMU started"
-end
+fi
 
 #
 # Start the sensors (depends on orb, px4io)
@@ -60,9 +60,7 @@ att_pos_estimator_ekf start
 # Load mixer and start controllers (depends on px4io)
 #
 mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
-fixedwing_backside start
+fw_pos_control_l1 start
+fw_att_control start
 
 echo "[HIL] setup done, running"
-
-# Exit shell to make it available to MAVLink
-exit 
diff --git a/makefiles/setup.mk b/makefiles/setup.mk
index 168e41a5c..42f9a8a7f 100644
--- a/makefiles/setup.mk
+++ b/makefiles/setup.mk
@@ -43,6 +43,7 @@
 #
 export PX4_INCLUDE_DIR	 = $(abspath $(PX4_BASE)/src/include)/
 export PX4_MODULE_SRC	 = $(abspath $(PX4_BASE)/src)/
+export PX4_LIB_DIR	 = $(abspath $(PX4_BASE)/src/lib)/
 export PX4_MK_DIR	 = $(abspath $(PX4_BASE)/makefiles)/
 export NUTTX_SRC	 = $(abspath $(PX4_BASE)/NuttX/nuttx)/
 export NUTTX_APP_SRC	 = $(abspath $(PX4_BASE)/NuttX/apps)/
@@ -57,7 +58,8 @@ export ARCHIVE_DIR	 = $(abspath $(PX4_BASE)/Archives)/
 #
 export INCLUDE_DIRS	:= $(PX4_MODULE_SRC) \
 			   $(PX4_MODULE_SRC)/modules/ \
-			   $(PX4_INCLUDE_DIR)
+			   $(PX4_INCLUDE_DIR) \
+			   $(PX4_LIB_DIR)
 
 #
 # Tools
diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp
index 57c256339..bb8d45bea 100644
--- a/src/drivers/hott/messages.cpp
+++ b/src/drivers/hott/messages.cpp
@@ -42,7 +42,7 @@
 #include <math.h>
 #include <stdio.h>
 #include <string.h>
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <unistd.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index efe7cf8eb..cf5f8d94c 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -54,7 +54,6 @@
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
-#include <systemlib/geo/geo.h>
 
 #include <nuttx/arch.h>
 #include <nuttx/clock.h>
@@ -178,6 +177,8 @@ static const int ERROR = -1;
 #define LSM303D_MAG_DEFAULT_RANGE_GA			2
 #define LSM303D_MAG_DEFAULT_RATE			100
 
+#define LSM303D_ONE_G					9.80665f
+
 extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); }
 
 
@@ -778,7 +779,7 @@ LSM303D::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCGRANGE:
 		/* convert to m/s^2 and return rounded in G */
-		return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f);
+		return (unsigned long)((_accel_range_m_s2)/LSM303D_ONE_G + 0.5f);
 
 	case ACCELIOCGSCALE:
 		/* copy scale out */
@@ -1015,27 +1016,27 @@ LSM303D::accel_set_range(unsigned max_g)
 		max_g = 16;
 
 	if (max_g <= 2) {
-		_accel_range_m_s2 = 2.0f*CONSTANTS_ONE_G;
+		_accel_range_m_s2 = 2.0f*LSM303D_ONE_G;
 		setbits |= REG2_FULL_SCALE_2G_A;
 		new_scale_g_digit = 0.061e-3f;
 
 	} else if (max_g <= 4) {
-		_accel_range_m_s2 = 4.0f*CONSTANTS_ONE_G;
+		_accel_range_m_s2 = 4.0f*LSM303D_ONE_G;
 		setbits |= REG2_FULL_SCALE_4G_A;
 		new_scale_g_digit = 0.122e-3f;
 
 	} else if (max_g <= 6) {
-		_accel_range_m_s2 = 6.0f*CONSTANTS_ONE_G;
+		_accel_range_m_s2 = 6.0f*LSM303D_ONE_G;
 		setbits |= REG2_FULL_SCALE_6G_A;
 		new_scale_g_digit = 0.183e-3f;
 
 	} else if (max_g <= 8) {
-		_accel_range_m_s2 = 8.0f*CONSTANTS_ONE_G;
+		_accel_range_m_s2 = 8.0f*LSM303D_ONE_G;
 		setbits |= REG2_FULL_SCALE_8G_A;
 		new_scale_g_digit = 0.244e-3f;
 
 	} else if (max_g <= 16) {
-		_accel_range_m_s2 = 16.0f*CONSTANTS_ONE_G;
+		_accel_range_m_s2 = 16.0f*LSM303D_ONE_G;
 		setbits |= REG2_FULL_SCALE_16G_A;
 		new_scale_g_digit = 0.732e-3f;
 
@@ -1043,7 +1044,7 @@ LSM303D::accel_set_range(unsigned max_g)
 		return -EINVAL;
 	}
 
-	_accel_range_scale = new_scale_g_digit * CONSTANTS_ONE_G;
+	_accel_range_scale = new_scale_g_digit * LSM303D_ONE_G;
 
 	modify_reg(ADDR_CTRL_REG2, clearbits, setbits);
 
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index 0e65923db..14f8f44b8 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -159,6 +159,8 @@
 
 #define MPU6000_DEFAULT_ONCHIP_FILTER_FREQ		42
 
+#define MPU6000_ONE_G					9.80665f
+
 class MPU6000_gyro;
 
 class MPU6000 : public device::SPI
@@ -543,8 +545,8 @@ void MPU6000::reset()
 
 	// Correct accel scale factors of 4096 LSB/g
 	// scale to m/s^2 ( 1g = 9.81 m/s^2)
-	_accel_range_scale = (CONSTANTS_ONE_G / 4096.0f);
-	_accel_range_m_s2 = 8.0f * CONSTANTS_ONE_G;
+	_accel_range_scale = (MPU6000_ONE_G / 4096.0f);
+	_accel_range_m_s2 = 8.0f * MPU6000_ONE_G;
 
 	usleep(1000);
 
@@ -906,7 +908,7 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 		// _accel_range_m_s2 = 8.0f * 9.81f;
 		return -EINVAL;
 	case ACCELIOCGRANGE:
-		return (unsigned long)((_accel_range_m_s2)/CONSTANTS_ONE_G + 0.5f);
+		return (unsigned long)((_accel_range_m_s2)/MPU6000_ONE_G + 0.5f);
 
 	case ACCELIOCSELFTEST:
 		return accel_self_test();
@@ -1409,7 +1411,7 @@ test()
 	warnx("acc  y:  \t%d\traw 0x%0x", (short)a_report.y_raw, (unsigned short)a_report.y_raw);
 	warnx("acc  z:  \t%d\traw 0x%0x", (short)a_report.z_raw, (unsigned short)a_report.z_raw);
 	warnx("acc range: %8.4f m/s^2 (%8.4f g)", (double)a_report.range_m_s2,
-	      (double)(a_report.range_m_s2 / CONSTANTS_ONE_G));
+	      (double)(a_report.range_m_s2 / MPU6000_ONE_G));
 
 	/* do a simple demand read */
 	sz = read(fd_gyro, &g_report, sizeof(g_report));
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index e29f76877..b286e0007 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -66,7 +66,7 @@
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/pid/pid.h>
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <systemlib/err.h>
diff --git a/src/modules/systemlib/geo/geo.c b/src/lib/geo/geo.c
index 6463e6489..63792dda5 100644
--- a/src/modules/systemlib/geo/geo.c
+++ b/src/lib/geo/geo.c
@@ -44,7 +44,7 @@
  * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <nuttx/config.h>
 #include <unistd.h>
 #include <pthread.h>
diff --git a/src/modules/systemlib/geo/geo.h b/src/lib/geo/geo.h
index dadec51ec..dadec51ec 100644
--- a/src/modules/systemlib/geo/geo.h
+++ b/src/lib/geo/geo.h
diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk
new file mode 100644
index 000000000..30a2dc99f
--- /dev/null
+++ b/src/lib/geo/module.mk
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+#
+# 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.
+#
+############################################################################
+
+#
+# Geo library
+#
+
+SRCS		 = geo.c
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
index 8f39acd9d..8f39acd9d 100644
--- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
+++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h
diff --git a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
index 181b7e433..181b7e433 100644
--- a/src/modules/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
+++ b/src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h
diff --git a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h
index 9c37ab4e5..9c37ab4e5 100644
--- a/src/modules/mathlib/CMSIS/Include/arm_common_tables.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_common_tables.h
diff --git a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h
index 406f737dc..406f737dc 100644
--- a/src/modules/mathlib/CMSIS/Include/arm_const_structs.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_const_structs.h
diff --git a/src/modules/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h
index 6f66f9ee3..6f66f9ee3 100644
--- a/src/modules/mathlib/CMSIS/Include/arm_math.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_math.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm3.h b/src/lib/mathlib/CMSIS/Include/core_cm3.h
index 8ac6dc078..8ac6dc078 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cm3.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cm3.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4.h b/src/lib/mathlib/CMSIS/Include/core_cm4.h
index 93efd3a7a..93efd3a7a 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cm4.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cm4.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h
index af1831ee1..af1831ee1 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cm4_simd.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cm4_simd.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h
index 139bc3c5e..139bc3c5e 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cmFunc.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cmFunc.h
diff --git a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h
index 8946c2c49..8946c2c49 100644
--- a/src/modules/mathlib/CMSIS/Include/core_cmInstr.h
+++ b/src/lib/mathlib/CMSIS/Include/core_cmInstr.h
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
index 6898bc27d..6898bc27d 100644
--- a/src/modules/mathlib/CMSIS/libarm_cortexM3l_math.a
+++ b/src/lib/mathlib/CMSIS/libarm_cortexM3l_math.a
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a
index a0185eaa9..a0185eaa9 100755
--- a/src/modules/mathlib/CMSIS/libarm_cortexM4l_math.a
+++ b/src/lib/mathlib/CMSIS/libarm_cortexM4l_math.a
diff --git a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
index 94525528e..94525528e 100755
--- a/src/modules/mathlib/CMSIS/libarm_cortexM4lf_math.a
+++ b/src/lib/mathlib/CMSIS/libarm_cortexM4lf_math.a
diff --git a/src/modules/mathlib/CMSIS/library.mk b/src/lib/mathlib/CMSIS/library.mk
index 0cc1b559d..0cc1b559d 100644
--- a/src/modules/mathlib/CMSIS/library.mk
+++ b/src/lib/mathlib/CMSIS/library.mk
diff --git a/src/modules/mathlib/CMSIS/license.txt b/src/lib/mathlib/CMSIS/license.txt
index 31afac1ec..31afac1ec 100644
--- a/src/modules/mathlib/CMSIS/license.txt
+++ b/src/lib/mathlib/CMSIS/license.txt
diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/lib/mathlib/math/Dcm.cpp
index f509f7081..f509f7081 100644
--- a/src/modules/mathlib/math/Dcm.cpp
+++ b/src/lib/mathlib/math/Dcm.cpp
diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/lib/mathlib/math/Dcm.hpp
index df8970d3a..df8970d3a 100644
--- a/src/modules/mathlib/math/Dcm.hpp
+++ b/src/lib/mathlib/math/Dcm.hpp
diff --git a/src/modules/mathlib/math/EulerAngles.cpp b/src/lib/mathlib/math/EulerAngles.cpp
index e733d23bb..e733d23bb 100644
--- a/src/modules/mathlib/math/EulerAngles.cpp
+++ b/src/lib/mathlib/math/EulerAngles.cpp
diff --git a/src/modules/mathlib/math/EulerAngles.hpp b/src/lib/mathlib/math/EulerAngles.hpp
index 399eecfa7..399eecfa7 100644
--- a/src/modules/mathlib/math/EulerAngles.hpp
+++ b/src/lib/mathlib/math/EulerAngles.hpp
diff --git a/src/modules/mathlib/math/Limits.cpp b/src/lib/mathlib/math/Limits.cpp
index d4c892d8a..d4c892d8a 100644
--- a/src/modules/mathlib/math/Limits.cpp
+++ b/src/lib/mathlib/math/Limits.cpp
diff --git a/src/modules/mathlib/math/Limits.hpp b/src/lib/mathlib/math/Limits.hpp
index fb778dd66..fb778dd66 100644
--- a/src/modules/mathlib/math/Limits.hpp
+++ b/src/lib/mathlib/math/Limits.hpp
diff --git a/src/modules/mathlib/math/Matrix.cpp b/src/lib/mathlib/math/Matrix.cpp
index ebd1aeda3..ebd1aeda3 100644
--- a/src/modules/mathlib/math/Matrix.cpp
+++ b/src/lib/mathlib/math/Matrix.cpp
diff --git a/src/modules/mathlib/math/Matrix.hpp b/src/lib/mathlib/math/Matrix.hpp
index f19db15ec..f19db15ec 100644
--- a/src/modules/mathlib/math/Matrix.hpp
+++ b/src/lib/mathlib/math/Matrix.hpp
diff --git a/src/modules/mathlib/math/Quaternion.cpp b/src/lib/mathlib/math/Quaternion.cpp
index 02fec4ca6..02fec4ca6 100644
--- a/src/modules/mathlib/math/Quaternion.cpp
+++ b/src/lib/mathlib/math/Quaternion.cpp
diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/lib/mathlib/math/Quaternion.hpp
index 048a55d33..048a55d33 100644
--- a/src/modules/mathlib/math/Quaternion.hpp
+++ b/src/lib/mathlib/math/Quaternion.hpp
diff --git a/src/modules/mathlib/math/Vector.cpp b/src/lib/mathlib/math/Vector.cpp
index 35158a396..35158a396 100644
--- a/src/modules/mathlib/math/Vector.cpp
+++ b/src/lib/mathlib/math/Vector.cpp
diff --git a/src/modules/mathlib/math/Vector.hpp b/src/lib/mathlib/math/Vector.hpp
index 73de793d5..73de793d5 100644
--- a/src/modules/mathlib/math/Vector.hpp
+++ b/src/lib/mathlib/math/Vector.hpp
diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/lib/mathlib/math/Vector2f.cpp
index 68e741817..68e741817 100644
--- a/src/modules/mathlib/math/Vector2f.cpp
+++ b/src/lib/mathlib/math/Vector2f.cpp
diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/lib/mathlib/math/Vector2f.hpp
index ecd62e81c..ecd62e81c 100644
--- a/src/modules/mathlib/math/Vector2f.hpp
+++ b/src/lib/mathlib/math/Vector2f.hpp
diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/lib/mathlib/math/Vector3.cpp
index dcb85600e..dcb85600e 100644
--- a/src/modules/mathlib/math/Vector3.cpp
+++ b/src/lib/mathlib/math/Vector3.cpp
diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/lib/mathlib/math/Vector3.hpp
index 568d9669a..568d9669a 100644
--- a/src/modules/mathlib/math/Vector3.hpp
+++ b/src/lib/mathlib/math/Vector3.hpp
diff --git a/src/modules/mathlib/math/arm/Matrix.cpp b/src/lib/mathlib/math/arm/Matrix.cpp
index 21661622a..21661622a 100644
--- a/src/modules/mathlib/math/arm/Matrix.cpp
+++ b/src/lib/mathlib/math/arm/Matrix.cpp
diff --git a/src/modules/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp
index 715fd3a5e..715fd3a5e 100644
--- a/src/modules/mathlib/math/arm/Matrix.hpp
+++ b/src/lib/mathlib/math/arm/Matrix.hpp
diff --git a/src/modules/mathlib/math/arm/Vector.cpp b/src/lib/mathlib/math/arm/Vector.cpp
index 7ea6496bb..7ea6496bb 100644
--- a/src/modules/mathlib/math/arm/Vector.cpp
+++ b/src/lib/mathlib/math/arm/Vector.cpp
diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp
index 4155800e8..4155800e8 100644
--- a/src/modules/mathlib/math/arm/Vector.hpp
+++ b/src/lib/mathlib/math/arm/Vector.hpp
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
index efb17225d..efb17225d 100644
--- a/src/modules/mathlib/math/filter/LowPassFilter2p.cpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.cpp
diff --git a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
index 208ec98d4..208ec98d4 100644
--- a/src/modules/mathlib/math/filter/LowPassFilter2p.hpp
+++ b/src/lib/mathlib/math/filter/LowPassFilter2p.hpp
diff --git a/src/modules/mathlib/math/filter/module.mk b/src/lib/mathlib/math/filter/module.mk
index fe92c8c70..fe92c8c70 100644
--- a/src/modules/mathlib/math/filter/module.mk
+++ b/src/lib/mathlib/math/filter/module.mk
diff --git a/src/modules/mathlib/math/generic/Matrix.cpp b/src/lib/mathlib/math/generic/Matrix.cpp
index 21661622a..21661622a 100644
--- a/src/modules/mathlib/math/generic/Matrix.cpp
+++ b/src/lib/mathlib/math/generic/Matrix.cpp
diff --git a/src/modules/mathlib/math/generic/Matrix.hpp b/src/lib/mathlib/math/generic/Matrix.hpp
index 5601a3447..5601a3447 100644
--- a/src/modules/mathlib/math/generic/Matrix.hpp
+++ b/src/lib/mathlib/math/generic/Matrix.hpp
diff --git a/src/modules/mathlib/math/generic/Vector.cpp b/src/lib/mathlib/math/generic/Vector.cpp
index 7ea6496bb..7ea6496bb 100644
--- a/src/modules/mathlib/math/generic/Vector.cpp
+++ b/src/lib/mathlib/math/generic/Vector.cpp
diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/lib/mathlib/math/generic/Vector.hpp
index 8cfdc676d..8cfdc676d 100644
--- a/src/modules/mathlib/math/generic/Vector.hpp
+++ b/src/lib/mathlib/math/generic/Vector.hpp
diff --git a/src/modules/mathlib/math/nasa_rotation_def.pdf b/src/lib/mathlib/math/nasa_rotation_def.pdf
index eb67a4bfc..eb67a4bfc 100644
--- a/src/modules/mathlib/math/nasa_rotation_def.pdf
+++ b/src/lib/mathlib/math/nasa_rotation_def.pdf
diff --git a/src/modules/mathlib/math/test/test.cpp b/src/lib/mathlib/math/test/test.cpp
index 2fa2f7e7c..2fa2f7e7c 100644
--- a/src/modules/mathlib/math/test/test.cpp
+++ b/src/lib/mathlib/math/test/test.cpp
diff --git a/src/modules/mathlib/math/test/test.hpp b/src/lib/mathlib/math/test/test.hpp
index 2027bb827..2027bb827 100644
--- a/src/modules/mathlib/math/test/test.hpp
+++ b/src/lib/mathlib/math/test/test.hpp
diff --git a/src/modules/mathlib/math/test_math.sce b/src/lib/mathlib/math/test_math.sce
index c3fba4729..c3fba4729 100644
--- a/src/modules/mathlib/math/test_math.sce
+++ b/src/lib/mathlib/math/test_math.sce
diff --git a/src/modules/mathlib/mathlib.h b/src/lib/mathlib/mathlib.h
index 40ffb22bc..40ffb22bc 100644
--- a/src/modules/mathlib/mathlib.h
+++ b/src/lib/mathlib/mathlib.h
diff --git a/src/modules/mathlib/module.mk b/src/lib/mathlib/module.mk
index 2146a1413..2146a1413 100644
--- a/src/modules/mathlib/module.mk
+++ b/src/lib/mathlib/module.mk
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index b6217a414..ddd2e23d9 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -116,7 +116,7 @@
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_accel.h>
-#include <systemlib/conversions.h>
+#include <geo/geo.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <mavlink/mavlink_log.h>
diff --git a/src/modules/controllib/uorb/blocks.hpp b/src/modules/controllib/uorb/blocks.hpp
index 9c0720aa5..46dc1bec2 100644
--- a/src/modules/controllib/uorb/blocks.hpp
+++ b/src/modules/controllib/uorb/blocks.hpp
@@ -58,7 +58,7 @@
 #include <poll.h>
 
 extern "C" {
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 }
 
 #include "../blocks.hpp"
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 3d3434670..f39bbaa72 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -279,6 +279,9 @@ int set_mavlink_interval_limit(struct mavlink_subscriptions *subs, int mavlink_m
 		orb_set_interval(subs->act_2_sub, min_interval);
 		orb_set_interval(subs->act_3_sub, min_interval);
 		orb_set_interval(subs->actuators_sub, min_interval);
+		orb_set_interval(subs->actuators_effective_sub, min_interval);
+		orb_set_interval(subs->spa_sub, min_interval);
+		orb_set_interval(mavlink_subs.rates_setpoint_sub, min_interval);
 		break;
 
 	case MAVLINK_MSG_ID_MANUAL_CONTROL:
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 01bbabd46..28f7af33c 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -50,6 +50,10 @@
 #include <mqueue.h>
 #include <string.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+#include <drivers/drv_baro.h>
 #include <time.h>
 #include <float.h>
 #include <unistd.h>
@@ -101,6 +105,10 @@ static orb_advert_t pub_hil_global_pos = -1;
 static orb_advert_t pub_hil_attitude = -1;
 static orb_advert_t pub_hil_gps = -1;
 static orb_advert_t pub_hil_sensors = -1;
+static orb_advert_t pub_hil_gyro = -1;
+static orb_advert_t pub_hil_accel = -1;
+static orb_advert_t pub_hil_mag = -1;
+static orb_advert_t pub_hil_baro = -1;
 static orb_advert_t pub_hil_airspeed = -1;
 
 static orb_advert_t cmd_pub = -1;
@@ -412,12 +420,12 @@ handle_message(mavlink_message_t *msg)
 
 			/* airspeed from differential pressure, ambient pressure and temp */
 			struct airspeed_s airspeed;
-			airspeed.timestamp = hrt_absolute_time();
 
 			float ias = calc_indicated_airspeed(imu.diff_pressure);
 			// XXX need to fix this
 			float tas = ias;
 
+			airspeed.timestamp = hrt_absolute_time();
 			airspeed.indicated_airspeed_m_s = ias;
 			airspeed.true_airspeed_m_s = tas;
 
@@ -428,7 +436,67 @@ handle_message(mavlink_message_t *msg)
 			}
 			//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
 
-			/* publish */
+			/* individual sensor publications */
+			struct gyro_report gyro;
+			gyro.x_raw = imu.xgyro / mrad2rad;
+			gyro.y_raw = imu.ygyro / mrad2rad;
+			gyro.z_raw = imu.zgyro / mrad2rad;
+			gyro.x = imu.xgyro;
+			gyro.y = imu.ygyro;
+			gyro.z = imu.zgyro;
+			gyro.temperature = imu.temperature;
+			gyro.timestamp = hrt_absolute_time();
+
+			if (pub_hil_gyro < 0) {
+				pub_hil_gyro = orb_advertise(ORB_ID(sensor_gyro), &gyro);
+			} else {
+				orb_publish(ORB_ID(sensor_gyro), pub_hil_gyro, &gyro);
+			}
+
+			struct accel_report accel;
+			accel.x_raw = imu.xacc / mg2ms2;
+			accel.y_raw = imu.yacc / mg2ms2;
+			accel.z_raw = imu.zacc / mg2ms2;
+			accel.x = imu.xacc;
+			accel.y = imu.yacc;
+			accel.z = imu.zacc;
+			accel.temperature = imu.temperature;
+			accel.timestamp = hrt_absolute_time();
+
+			if (pub_hil_accel < 0) {
+				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+			} else {
+				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+			}
+
+			struct mag_report mag;
+			mag.x_raw = imu.xmag / mga2ga;
+			mag.y_raw = imu.ymag / mga2ga;
+			mag.z_raw = imu.zmag / mga2ga;
+			mag.x = imu.xmag;
+			mag.y = imu.ymag;
+			mag.z = imu.zmag;
+			mag.timestamp = hrt_absolute_time();
+
+			if (pub_hil_mag < 0) {
+				pub_hil_mag = orb_advertise(ORB_ID(sensor_mag), &mag);
+			} else {
+				orb_publish(ORB_ID(sensor_mag), pub_hil_mag, &mag);
+			}
+
+			struct baro_report baro;
+			baro.pressure = imu.abs_pressure;
+			baro.altitude = imu.pressure_alt;
+			baro.temperature = imu.temperature;
+			baro.timestamp = hrt_absolute_time();
+
+			if (pub_hil_baro < 0) {
+				pub_hil_baro = orb_advertise(ORB_ID(sensor_baro), &baro);
+			} else {
+				orb_publish(ORB_ID(sensor_baro), pub_hil_baro, &baro);
+			}
+
+			/* publish combined sensor topic */
 			if (pub_hil_sensors > 0) {
 				orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
 			} else {
@@ -552,6 +620,22 @@ handle_message(mavlink_message_t *msg)
 			} else {
 				pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
 			}
+
+			struct accel_report accel;
+			accel.x_raw = hil_state.xacc / 9.81f * 1e3f;
+			accel.y_raw = hil_state.yacc / 9.81f * 1e3f;
+			accel.z_raw = hil_state.zacc / 9.81f * 1e3f;
+			accel.x = hil_state.xacc;
+			accel.y = hil_state.yacc;
+			accel.z = hil_state.zacc;
+			accel.temperature = 25.0f;
+			accel.timestamp = hrt_absolute_time();
+
+			if (pub_hil_accel < 0) {
+				pub_hil_accel = orb_advertise(ORB_ID(sensor_accel), &accel);
+			} else {
+				orb_publish(ORB_ID(sensor_accel), pub_hil_accel, &accel);
+			}
 		}
 
 		if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 2a260861d..c711b1803 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -71,7 +71,8 @@ struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
-struct actuator_controls_effective_s actuators_0;
+struct actuator_controls_effective_s actuators_effective_0;
+struct actuator_controls_s actuators_0;
 struct vehicle_attitude_s att;
 
 struct mavlink_subscriptions mavlink_subs;
@@ -112,6 +113,7 @@ static void	l_attitude_setpoint(const struct listener *l);
 static void	l_actuator_outputs(const struct listener *l);
 static void	l_actuator_armed(const struct listener *l);
 static void	l_manual_control_setpoint(const struct listener *l);
+static void	l_vehicle_attitude_controls_effective(const struct listener *l);
 static void	l_vehicle_attitude_controls(const struct listener *l);
 static void	l_debug_key_value(const struct listener *l);
 static void	l_optical_flow(const struct listener *l);
@@ -138,6 +140,7 @@ static const struct listener listeners[] = {
 	{l_actuator_armed,		&mavlink_subs.armed_sub,	0},
 	{l_manual_control_setpoint,	&mavlink_subs.man_control_sp_sub, 0},
 	{l_vehicle_attitude_controls,	&mavlink_subs.actuators_sub,	0},
+	{l_vehicle_attitude_controls_effective,	&mavlink_subs.actuators_effective_sub,	0},
 	{l_debug_key_value,		&mavlink_subs.debug_key_value,	0},
 	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
 	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
@@ -567,28 +570,54 @@ l_manual_control_setpoint(const struct listener *l)
 }
 
 void
-l_vehicle_attitude_controls(const struct listener *l)
+l_vehicle_attitude_controls_effective(const struct listener *l)
 {
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0);
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_effective_sub, &actuators_effective_0);
 
 	if (gcs_link) {
 		/* send, add spaces so that string buffer is at least 10 chars long */
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl0    ",
-						   actuators_0.control_effective[0]);
+						   actuators_effective_0.control_effective[0]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl1    ",
-						   actuators_0.control_effective[1]);
+						   actuators_effective_0.control_effective[1]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl2     ",
-						   actuators_0.control_effective[2]);
+						   actuators_effective_0.control_effective[2]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl3     ",
-						   actuators_0.control_effective[3]);
+						   actuators_effective_0.control_effective[3]);
+	}
+}
+
+void
+l_vehicle_attitude_controls(const struct listener *l)
+{
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, mavlink_subs.actuators_sub, &actuators_0);
+
+	if (gcs_link) {
+		/* send, add spaces so that string buffer is at least 10 chars long */
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl0    ",
+						   actuators_0.control[0]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl1    ",
+						   actuators_0.control[1]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl2     ",
+						   actuators_0.control[2]);
+		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
+						   last_sensor_timestamp / 1000,
+						   "ctrl3     ",
+						   actuators_0.control[3]);
 	}
 }
 
@@ -637,7 +666,7 @@ l_airspeed(const struct listener *l)
 	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
 
 	float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
-	float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1);
+	float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
 	float alt = global_pos.alt;
 	float climb = global_pos.vz;
 
@@ -773,7 +802,10 @@ uorb_receive_start(void)
 	orb_set_interval(mavlink_subs.man_control_sp_sub, 100);	/* 10Hz updates */
 
 	/* --- ACTUATOR CONTROL VALUE --- */
-	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	mavlink_subs.actuators_effective_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	orb_set_interval(mavlink_subs.actuators_effective_sub, 100);	/* 10Hz updates */
+
+	mavlink_subs.actuators_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	orb_set_interval(mavlink_subs.actuators_sub, 100);	/* 10Hz updates */
 
 	/* --- DEBUG VALUE OUTPUT --- */
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index 506f73105..e2e630046 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -80,6 +80,7 @@ struct mavlink_subscriptions {
 	int safety_sub;
 	int actuators_sub;
 	int armed_sub;
+	int actuators_effective_sub;
 	int local_pos_sub;
 	int spa_sub;
 	int spl_sub;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 3466841c4..760e598bc 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -64,9 +64,8 @@
 #include <mavlink/mavlink_log.h>
 #include <poll.h>
 #include <systemlib/err.h>
-#include <systemlib/geo/geo.h>
+#include <geo/geo.h>
 #include <systemlib/systemlib.h>
-#include <systemlib/conversions.h>
 #include <drivers/drv_hrt.h>
 
 #include "position_estimator_inav_params.h"
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/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 \