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

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 

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

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>

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);
 

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));

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>

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>

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