Merge branch 'master' into vector_control

src/lib/conversion/module.mk

@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (C) 2013 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.
+#
+############################################################################
+
+#
+# Conversion library
+#
+
+SRCS		 = rotation.cpp

src/lib/conversion/rotation.cpp

@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rotation.cpp
+ *
+ * Vector rotation library
+ */
+
+#include "math.h"
+#include "rotation.h"
+
+__EXPORT void
+get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
+{
+	/* first set to zero */
+	rot_matrix->Matrix::zero(3, 3);
+
+	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
+	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
+	float yaw   = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
+
+	math::EulerAngles euler(roll, pitch, yaw);
+
+	math::Dcm R(euler);
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			(*rot_matrix)(i, j) = R(i, j);
+		}
+	}
+}

src/lib/conversion/rotation.h

@@ -0,0 +1,121 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rotation.h
+ *
+ * Vector rotation library
+ */
+
+#ifndef ROTATION_H_
+#define ROTATION_H_
+
+#include <unistd.h>
+#include <mathlib/mathlib.h>
+
+/**
+ * Enum for board and external compass rotations.
+ * This enum maps from board attitude to airframe attitude.
+ */
+enum Rotation {
+	ROTATION_NONE                = 0,
+	ROTATION_YAW_45              = 1,
+	ROTATION_YAW_90              = 2,
+	ROTATION_YAW_135             = 3,
+	ROTATION_YAW_180             = 4,
+	ROTATION_YAW_225             = 5,
+	ROTATION_YAW_270             = 6,
+	ROTATION_YAW_315             = 7,
+	ROTATION_ROLL_180            = 8,
+	ROTATION_ROLL_180_YAW_45     = 9,
+	ROTATION_ROLL_180_YAW_90     = 10,
+	ROTATION_ROLL_180_YAW_135    = 11,
+	ROTATION_PITCH_180           = 12,
+	ROTATION_ROLL_180_YAW_225    = 13,
+	ROTATION_ROLL_180_YAW_270    = 14,
+	ROTATION_ROLL_180_YAW_315    = 15,
+	ROTATION_ROLL_90             = 16,
+	ROTATION_ROLL_90_YAW_45      = 17,
+	ROTATION_ROLL_90_YAW_90      = 18,
+	ROTATION_ROLL_90_YAW_135     = 19,
+	ROTATION_ROLL_270            = 20,
+	ROTATION_ROLL_270_YAW_45     = 21,
+	ROTATION_ROLL_270_YAW_90     = 22,
+	ROTATION_ROLL_270_YAW_135    = 23,
+	ROTATION_PITCH_90            = 24,
+	ROTATION_PITCH_270           = 25,
+	ROTATION_MAX
+};
+
+typedef struct {
+	uint16_t roll;
+	uint16_t pitch;
+	uint16_t yaw;
+} rot_lookup_t;
+
+const rot_lookup_t rot_lookup[] = {
+	{  0,   0,   0 },
+	{  0,   0,  45 },
+	{  0,   0,  90 },
+	{  0,   0, 135 },
+	{  0,   0, 180 },
+	{  0,   0, 225 },
+	{  0,   0, 270 },
+	{  0,   0, 315 },
+	{180,   0,   0 },
+	{180,   0,  45 },
+	{180,   0,  90 },
+	{180,   0, 135 },
+	{  0, 180,   0 },
+	{180,   0, 225 },
+	{180,   0, 270 },
+	{180,   0, 315 },
+	{ 90,   0,   0 },
+	{ 90,   0,  45 },
+	{ 90,   0,  90 },
+	{ 90,   0, 135 },
+	{270,   0,   0 },
+	{270,   0,  45 },
+	{270,   0,  90 },
+	{270,   0, 135 },
+	{  0,  90,   0 },
+	{  0, 270,   0 }
+};
+
+/**
+ * Get the rotation matrix
+ */
+__EXPORT void
+get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+
+#endif /* ROTATION_H_ */

src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp

@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.cpp
+ * Implementation of a simple orthogonal pitch PID controller.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "ecl_pitch_controller.h"
+#include <math.h>
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+
+ECL_PitchController::ECL_PitchController() :
+	_last_run(0),
+	_last_output(0.0f),
+	_integrator(0.0f),
+	_rate_error(0.0f),
+	_rate_setpoint(0.0f),
+	_max_deflection_rad(math::radians(45.0f))
+{
+}
+
+float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler,
+				   bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+{
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
+
+	float k_roll_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_i_rate = _k_i * _tc;
+
+	/* input conditioning */
+	if (!isfinite(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+		airspeed = airspeed_min;
+	}
+
+	/* flying inverted (wings upside down) ? */
+	bool inverted = false;
+
+	/* roll is used as feedforward term and inverted flight needs to be considered */
+	if (fabsf(roll) < math::radians(90.0f)) {
+		/* not inverted, but numerically still potentially close to infinity */
+		roll = math::constrain(roll, math::radians(-80.0f), math::radians(80.0f));
+	} else {
+		/* inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity */
+
+		/* note: the ranges are extended by 10 deg here to avoid numeric resolution effects */
+		if (roll > 0.0f) {
+			/* right hemisphere */
+			roll = math::constrain(roll, math::radians(100.0f), math::radians(180.0f));
+		} else {
+			/* left hemisphere */
+			roll = math::constrain(roll, math::radians(-100.0f), math::radians(-180.0f));
+		}
+	}
+
+	/* calculate the offset in the rate resulting from rolling  */
+	float turn_offset = fabsf((CONSTANTS_ONE_G / airspeed) *
+				tanf(roll) * sinf(roll)) * _roll_ff;
+	if (inverted)
+		turn_offset = -turn_offset;
+
+	float pitch_error = pitch_setpoint - pitch;
+	/* rate setpoint from current error and time constant */
+	_rate_setpoint = pitch_error / _tc;
+
+	/* add turn offset */
+	_rate_setpoint += turn_offset;
+
+	_rate_error = _rate_setpoint - pitch_rate;
+
+	float ilimit_scaled = _integrator_max * scaler;
+
+	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+		float id = _rate_error * k_i_rate * dt * scaler;
+
+		/*
+		 * anti-windup: do not allow integrator to increase into the
+		 * wrong direction if actuator is at limit
+		 */
+		if (_last_output < -_max_deflection_rad) {
+			/* only allow motion to center: increase value */
+			id = math::max(id, 0.0f);
+		} else if (_last_output > _max_deflection_rad) {
+			/* only allow motion to center: decrease value */
+			id = math::min(id, 0.0f);
+		}
+
+		_integrator += id;
+	}
+
+	/* integrator limit */
+	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
+	/* store non-limited output */
+	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_roll_ff)) * scaler;
+
+	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+}
+
+void ECL_PitchController::reset_integrator()
+{
+	_integrator = 0.0f;
+}

src/lib/ecl/attitude_fw/ecl_pitch_controller.h

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_pitch_controller.h
+ * Definition of a simple orthogonal pitch PID controller.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
+ */
+
+#ifndef ECL_PITCH_CONTROLLER_H
+#define ECL_PITCH_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+class __EXPORT ECL_PitchController
+{
+public:
+	ECL_PitchController();
+
+	float control(float pitch_setpoint, float pitch, float pitch_rate, float roll, float scaler = 1.0f,
+		      bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+
+	void reset_integrator();
+
+	void set_time_constant(float time_constant) {
+		_tc = time_constant;
+	}
+	void set_k_p(float k_p) {
+		_k_p = k_p;
+	}
+	void set_k_i(float k_i) {
+		_k_i = k_i;
+	}
+	void set_k_d(float k_d) {
+		_k_d = k_d;
+	}
+	void set_integrator_max(float max) {
+		_integrator_max = max;
+	}
+	void set_max_rate_pos(float max_rate_pos) {
+		_max_rate_pos = max_rate_pos;
+	}
+	void set_max_rate_neg(float max_rate_neg) {
+		_max_rate_neg = max_rate_neg;
+	}
+	void set_roll_ff(float roll_ff) {
+		_roll_ff = roll_ff;
+	}
+
+	float get_rate_error() {
+		return _rate_error;
+	}
+
+	float get_desired_rate() {
+		return _rate_setpoint;
+	}
+
+private:
+
+	uint64_t _last_run;
+	float _tc;
+	float _k_p;
+	float _k_i;
+	float _k_d;
+	float _integrator_max;
+	float _max_rate_pos;
+	float _max_rate_neg;
+	float _roll_ff;
+	float _last_output;
+	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _max_deflection_rad;
+};
+
+#endif // ECL_PITCH_CONTROLLER_H

src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.cpp
+ * Implementation of a simple orthogonal roll PID controller.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "../ecl.h"
+#include "ecl_roll_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+
+ECL_RollController::ECL_RollController() :
+	_last_run(0),
+	_tc(0.1f),
+	_last_output(0.0f),
+	_integrator(0.0f),
+	_rate_error(0.0f),
+	_rate_setpoint(0.0f),
+	_max_deflection_rad(math::radians(45.0f))
+{
+
+}
+
+float ECL_RollController::control(float roll_setpoint, float roll, float roll_rate,
+				  float scaler, bool lock_integrator, float airspeed_min, float airspeed_max, float airspeed)
+{
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
+
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
+
+	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
+	float k_i_rate = _k_i * _tc;
+
+	/* input conditioning */
+	if (!isfinite(airspeed)) {
+		/* airspeed is NaN, +- INF or not available, pick center of band */
+		airspeed = 0.5f * (airspeed_min + airspeed_max);
+	} else if (airspeed < airspeed_min) {
+		airspeed = airspeed_min;
+	}
+
+	float roll_error = roll_setpoint - roll;
+	_rate_setpoint = roll_error / _tc;
+
+	/* limit the rate */
+	if (_max_rate > 0.01f) {
+		_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+		_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+	}
+
+	_rate_error = _rate_setpoint - roll_rate;
+
+
+	float ilimit_scaled = _integrator_max * scaler;
+
+	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
+
+		float id = _rate_error * k_i_rate * dt * scaler;
+
+		/*
+		 * anti-windup: do not allow integrator to increase into the
+		 * wrong direction if actuator is at limit
+		 */
+		if (_last_output < -_max_deflection_rad) {
+			/* only allow motion to center: increase value */
+			id = math::max(id, 0.0f);
+		} else if (_last_output > _max_deflection_rad) {
+			/* only allow motion to center: decrease value */
+			id = math::min(id, 0.0f);
+		}
+
+		_integrator += id;
+	}
+
+	/* integrator limit */
+	_integrator = math::constrain(_integrator, -ilimit_scaled, ilimit_scaled);
+	/* store non-limited output */
+	_last_output = ((_rate_error * _k_d * scaler) + _integrator + (_rate_setpoint * k_ff)) * scaler;
+
+	return math::constrain(_last_output, -_max_deflection_rad, _max_deflection_rad);
+}
+
+void ECL_RollController::reset_integrator()
+{
+	_integrator = 0.0f;
+}
+

src/lib/ecl/attitude_fw/ecl_roll_controller.h

@@ -0,0 +1,108 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_roll_controller.h
+ * Definition of a simple orthogonal roll PID controller.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Acknowledgements:
+ *
+ *   The control design is based on a design
+ *   by Paul Riseborough and Andrew Tridgell, 2013,
+ *   which in turn is based on initial work of
+ *   Jonathan Challinger, 2012.
+ */
+
+#ifndef ECL_ROLL_CONTROLLER_H
+#define ECL_ROLL_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+class __EXPORT ECL_RollController
+{
+public:
+	ECL_RollController();
+
+	float control(float roll_setpoint, float roll, float roll_rate,
+		      float scaler = 1.0f, bool lock_integrator = false, float airspeed_min = 0.0f, float airspeed_max = 0.0f, float airspeed = (0.0f / 0.0f));
+
+	void reset_integrator();
+
+	void set_time_constant(float time_constant) {
+		if (time_constant > 0.1f && time_constant < 3.0f) {
+			_tc = time_constant;
+		}
+	}
+	void set_k_p(float k_p) {
+		_k_p = k_p;
+	}
+	void set_k_i(float k_i) {
+		_k_i = k_i;
+	}
+	void set_k_d(float k_d) {
+		_k_d = k_d;
+	}
+	void set_integrator_max(float max) {
+		_integrator_max = max;
+	}
+	void set_max_rate(float max_rate) {
+		_max_rate = max_rate;
+	}
+
+	float get_rate_error() {
+		return _rate_error;
+	}
+
+	float get_desired_rate() {
+		return _rate_setpoint;
+	}
+
+private:
+	uint64_t _last_run;
+	float _tc;
+	float _k_p;
+	float _k_i;
+	float _k_d;
+	float _integrator_max;
+	float _max_rate;
+	float _last_output;
+	float _integrator;
+	float _rate_error;
+	float _rate_setpoint;
+	float _max_deflection_rad;
+};
+
+#endif // ECL_ROLL_CONTROLLER_H

src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp

@@ -0,0 +1,75 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.cpp
+ * Implementation of a simple orthogonal coordinated turn yaw PID controller.
+ *
+ * Authors and acknowledgements in header.
+ */
+
+#include "ecl_yaw_controller.h"
+#include <stdint.h>
+#include <float.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+#include <mathlib/mathlib.h>
+
+ECL_YawController::ECL_YawController() :
+	_last_run(0),
+	_last_error(0.0f),
+	_last_output(0.0f),
+	_last_rate_hp_out(0.0f),
+	_last_rate_hp_in(0.0f),
+	_k_d_last(0.0f),
+	_integrator(0.0f)
+{
+
+}
+
+float ECL_YawController::control(float roll, float yaw_rate, float accel_y, float scaler, bool lock_integrator,
+				 float airspeed_min, float airspeed_max, float aspeed)
+{
+	/* get the usual dt estimate */
+	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
+	_last_run = ecl_absolute_time();
+
+	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
+
+	return 0.0f;
+}
+
+void ECL_YawController::reset_integrator()
+{
+	_integrator = 0.0f;
+}

src/lib/ecl/attitude_fw/ecl_yaw_controller.h

@@ -0,0 +1,89 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_yaw_controller.h
+ * Definition of a simple orthogonal coordinated turn yaw PID controller.
+ *
+ */
+#ifndef ECL_YAW_CONTROLLER_H
+#define ECL_YAW_CONTROLLER_H
+
+#include <stdbool.h>
+#include <stdint.h>
+
+class __EXPORT ECL_YawController
+{
+public:
+	ECL_YawController();
+
+	float control(float roll, float yaw_rate, float accel_y, float scaler = 1.0f, bool lock_integrator = false,
+		      float airspeed_min = 0, float airspeed_max = 0, float aspeed = (0.0f / 0.0f));
+
+	void reset_integrator();
+
+	void set_k_side(float k_a) {
+		_k_side = k_a;
+	}
+	void set_k_i(float k_i) {
+		_k_i = k_i;
+	}
+	void set_k_d(float k_d) {
+		_k_d = k_d;
+	}
+	void set_k_roll_ff(float k_roll_ff) {
+		_k_roll_ff = k_roll_ff;
+	}
+	void set_integrator_max(float max) {
+		_integrator_max = max;
+	}
+
+private:
+	uint64_t _last_run;
+
+	float _k_side;
+	float _k_i;
+	float _k_d;
+	float _k_roll_ff;
+	float _integrator_max;
+
+	float _last_error;
+	float _last_output;
+	float _last_rate_hp_out;
+	float _last_rate_hp_in;
+	float _k_d_last;
+	float _integrator;
+
+};
+
+#endif // ECL_YAW_CONTROLLER_H

src/lib/ecl/ecl.h

@@ -0,0 +1,44 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 APL 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 ecl.h
+ * Adapter / shim layer for system calls needed by ECL
+ *
+ */
+
+#include <drivers/drv_hrt.h>
+#include <geo/geo.h>
+
+#define ecl_absolute_time hrt_absolute_time
+#define ecl_elapsed_time hrt_elapsed_time

src/lib/ecl/l1/ecl_l1_pos_controller.cpp

@@ -0,0 +1,366 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_controller.h
+ * Implementation of L1 position control.
+ * Authors and acknowledgements in header.
+ *
+ */
+
+#include "ecl_l1_pos_controller.h"
+
+float ECL_L1_Pos_Controller::nav_roll()
+{
+	float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
+	ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad);
+	return ret;
+}
+
+float ECL_L1_Pos_Controller::nav_lateral_acceleration_demand()
+{
+	return _lateral_accel;
+}
+
+float ECL_L1_Pos_Controller::nav_bearing()
+{
+	return _wrap_pi(_nav_bearing);
+}
+
+float ECL_L1_Pos_Controller::bearing_error()
+{
+	return _bearing_error;
+}
+
+float ECL_L1_Pos_Controller::target_bearing()
+{
+	return _target_bearing;
+}
+
+float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
+{
+	/* following [2], switching on L1 distance */
+	return math::max(wp_radius, _L1_distance);
+}
+
+bool ECL_L1_Pos_Controller::reached_loiter_target(void)
+{
+	return _circle_mode;
+}
+
+float ECL_L1_Pos_Controller::crosstrack_error(void)
+{
+	return _crosstrack_error;
+}
+
+void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
+				       const math::Vector2f &ground_speed_vector)
+{
+
+	/* this follows the logic presented in [1] */
+
+	float eta;
+	float xtrack_vel;
+	float ltrack_vel;
+
+	/* get the direction between the last (visited) and next waypoint */
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_B.getX(), vector_B.getY());
+
+	/* enforce a minimum ground speed of 0.1 m/s to avoid singularities */
+	float ground_speed = math::max(ground_speed_vector.length(), 0.1f);
+
+	/* calculate the L1 length required for the desired period */
+	_L1_distance = _L1_ratio * ground_speed;
+
+	/* calculate vector from A to B */
+	math::Vector2f vector_AB = get_local_planar_vector(vector_A, vector_B);
+
+	/*
+	 * check if waypoints are on top of each other. If yes,
+	 * skip A and directly continue to B
+	 */
+	if (vector_AB.length() < 1.0e-6f) {
+		vector_AB = get_local_planar_vector(vector_curr_position, vector_B);
+	}
+
+	vector_AB.normalize();
+
+	/* calculate the vector from waypoint A to the aircraft */
+	math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+
+	/* calculate crosstrack error (output only) */
+	_crosstrack_error = vector_AB % vector_A_to_airplane;
+
+	/*
+	 * If the current position is in a +-135 degree angle behind waypoint A
+	 * and further away from A than the L1 distance, then A becomes the L1 point.
+	 * If the aircraft is already between A and B normal L1 logic is applied.
+	 */
+	float distance_A_to_airplane = vector_A_to_airplane.length();
+	float alongTrackDist = vector_A_to_airplane * vector_AB;
+
+	/* estimate airplane position WRT to B */
+	math::Vector2f vector_B_to_P_unit = get_local_planar_vector(vector_B, vector_curr_position).normalized();
+	
+	/* calculate angle of airplane position vector relative to line) */
+
+	// XXX this could probably also be based solely on the dot product
+	float AB_to_BP_bearing = atan2f(vector_B_to_P_unit % vector_AB, vector_B_to_P_unit * vector_AB);
+
+	/* extension from [2], fly directly to A */
+	if (distance_A_to_airplane > _L1_distance && alongTrackDist / math::max(distance_A_to_airplane , 1.0f) < -0.7071f) {
+
+		/* calculate eta to fly to waypoint A */
+
+		/* unit vector from waypoint A to current position */
+		math::Vector2f vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % (-vector_A_to_airplane_unit);
+		/* velocity along line */
+		ltrack_vel = ground_speed_vector * (-vector_A_to_airplane_unit);
+		eta = atan2f(xtrack_vel, ltrack_vel);
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+
+	/*
+	 * If the AB vector and the vector from B to airplane point in the same
+	 * direction, we have missed the waypoint. At +- 90 degrees we are just passing it.
+	 */
+	} else if (fabsf(AB_to_BP_bearing) < math::radians(100.0f)) {
+		/*
+		 * Extension, fly back to waypoint.
+		 * 
+		 * This corner case is possible if the system was following
+		 * the AB line from waypoint A to waypoint B, then is
+		 * switched to manual mode (or otherwise misses the waypoint)
+		 * and behind the waypoint continues to follow the AB line.
+		 */
+
+		/* calculate eta to fly to waypoint B */
+		
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % (-vector_B_to_P_unit);
+		/* velocity along line */
+		ltrack_vel = ground_speed_vector * (-vector_B_to_P_unit);
+		eta = atan2f(xtrack_vel, ltrack_vel);
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_B_to_P_unit.getY() , -vector_B_to_P_unit.getX());
+
+	} else {
+
+		/* calculate eta to fly along the line between A and B */
+
+		/* velocity across / orthogonal to line */
+		xtrack_vel = ground_speed_vector % vector_AB;
+		/* velocity along line */
+		ltrack_vel = ground_speed_vector * vector_AB;
+		/* calculate eta2 (angle of velocity vector relative to line) */
+		float eta2 = atan2f(xtrack_vel, ltrack_vel);
+		/* calculate eta1 (angle to L1 point) */
+		float xtrackErr = vector_A_to_airplane % vector_AB;
+		float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f);
+		/* limit output to 45 degrees */
+		sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
+		float eta1 = asinf(sine_eta1);
+		eta = eta1 + eta2;
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(vector_AB.getY(), vector_AB.getX()) + eta1;
+
+	}
+
+	/* limit angle to +-90 degrees */
+	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
+	_lateral_accel = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
+
+	/* flying to waypoints, not circling them */
+	_circle_mode = false;
+
+	/* the bearing angle, in NED frame */
+	_bearing_error = eta;
+}
+
+void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
+				       const math::Vector2f &ground_speed_vector)
+{
+	/* the complete guidance logic in this section was proposed by [2] */
+
+	/* calculate the gains for the PD loop (circle tracking) */
+	float omega = (2.0f * M_PI_F / _L1_period);
+	float K_crosstrack = omega * omega;
+	float K_velocity = 2.0f * _L1_damping * omega;
+
+	/* update bearing to next waypoint */
+	_target_bearing = get_bearing_to_next_waypoint(vector_curr_position.getX(), vector_curr_position.getY(), vector_A.getX(), vector_A.getY());
+
+	/* ground speed, enforce minimum of 0.1 m/s to avoid singularities */
+	float ground_speed = math::max(ground_speed_vector.length() , 0.1f);
+
+	/* calculate the L1 length required for the desired period */
+	_L1_distance = _L1_ratio * ground_speed;
+
+	/* calculate the vector from waypoint A to current position */
+	math::Vector2f vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
+
+	/* store the normalized vector from waypoint A to current position */
+	math::Vector2f vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+
+	/* calculate eta angle towards the loiter center */
+
+	/* velocity across / orthogonal to line from waypoint to current position */
+	float xtrack_vel_center = vector_A_to_airplane_unit % ground_speed_vector;
+	/* velocity along line from waypoint to current position */
+	float ltrack_vel_center = - (ground_speed_vector * vector_A_to_airplane_unit);
+	float eta = atan2f(xtrack_vel_center, ltrack_vel_center);
+	/* limit eta to 90 degrees */
+	eta = math::constrain(eta, -M_PI_F / 2.0f, +M_PI_F / 2.0f);
+
+	/* calculate the lateral acceleration to capture the center point */
+	float lateral_accel_sp_center = _K_L1 * ground_speed * ground_speed / _L1_distance * sinf(eta);
+
+	/* for PD control: Calculate radial position and velocity errors */
+
+	/* radial velocity error */
+	float xtrack_vel_circle = -ltrack_vel_center;
+	/* radial distance from the loiter circle (not center) */
+	float xtrack_err_circle = vector_A_to_airplane.length() - radius;
+
+	/* cross track error for feedback */
+	_crosstrack_error = xtrack_err_circle;
+
+	/* calculate PD update to circle waypoint */
+	float lateral_accel_sp_circle_pd = (xtrack_err_circle * K_crosstrack + xtrack_vel_circle * K_velocity);
+
+	/* calculate velocity on circle / along tangent */
+	float tangent_vel = xtrack_vel_center * loiter_direction;
+
+	/* prevent PD output from turning the wrong way */
+	if (tangent_vel < 0.0f) {
+		lateral_accel_sp_circle_pd = math::max(lateral_accel_sp_circle_pd , 0.0f);
+	}
+
+	/* calculate centripetal acceleration setpoint */
+	float lateral_accel_sp_circle_centripetal = tangent_vel * tangent_vel / math::max((0.5f * radius) , (radius + xtrack_err_circle));
+
+	/* add PD control on circle and centripetal acceleration for total circle command */
+	float lateral_accel_sp_circle = loiter_direction * (lateral_accel_sp_circle_pd + lateral_accel_sp_circle_centripetal);
+
+	/*
+	 * Switch between circle (loiter) and capture (towards waypoint center) mode when
+	 * the commands switch over. Only fly towards waypoint if outside the circle.
+	 */
+
+	// XXX check switch over
+	if ((lateral_accel_sp_center < lateral_accel_sp_circle && loiter_direction > 0 && xtrack_err_circle > 0.0f) |
+		(lateral_accel_sp_center > lateral_accel_sp_circle && loiter_direction < 0 && xtrack_err_circle > 0.0f)) {
+		_lateral_accel = lateral_accel_sp_center;
+		_circle_mode = false;
+		/* angle between requested and current velocity vector */
+		_bearing_error = eta;
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+
+	} else {
+		_lateral_accel = lateral_accel_sp_circle;
+		_circle_mode = true;
+		_bearing_error = 0.0f;
+		/* bearing from current position to L1 point */
+		_nav_bearing = atan2f(-vector_A_to_airplane_unit.getY() , -vector_A_to_airplane_unit.getX());
+	}
+}
+
+
+void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed_vector)
+{
+	/* the complete guidance logic in this section was proposed by [2] */
+
+	float eta;
+
+	/* 
+	 * As the commanded heading is the only reference
+	 * (and no crosstrack correction occurs),
+	 * target and navigation bearing become the same
+	 */
+	_target_bearing = _nav_bearing = _wrap_pi(navigation_heading);
+	eta = _target_bearing - _wrap_pi(current_heading);
+	eta = _wrap_pi(eta);
+
+	/* consequently the bearing error is exactly eta: */
+	_bearing_error = eta; 
+
+	/* ground speed is the length of the ground speed vector */
+	float ground_speed = ground_speed_vector.length();
+
+	/* adjust L1 distance to keep constant frequency */
+	_L1_distance = ground_speed / _heading_omega;
+	float omega_vel = ground_speed * _heading_omega;
+
+	/* not circling a waypoint */
+	_circle_mode = false;
+
+	/* navigating heading means by definition no crosstrack error */
+	_crosstrack_error = 0;
+
+	/* limit eta to 90 degrees */
+	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
+	_lateral_accel = 2.0f * sinf(eta) * omega_vel;
+}
+
+void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
+{
+	/* the logic in this section is trivial, but originally proposed by [2] */
+
+	/* reset all heading / error measures resulting in zero roll */
+	_target_bearing = current_heading;
+	_nav_bearing = current_heading;
+	_bearing_error = 0;
+	_crosstrack_error = 0;
+	_lateral_accel = 0;
+
+	/* not circling a waypoint when flying level */
+	_circle_mode = false;
+
+}
+
+
+math::Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const
+{
+	/* this is an approximation for small angles, proposed by [2] */
+
+	math::Vector2f out;
+
+	out.setX(math::radians((target.getX() - origin.getX())));
+	out.setY(math::radians((target.getY() - origin.getY())*cosf(math::radians(origin.getX()))));
+
+	return out * static_cast<float>(CONSTANTS_RADIUS_OF_EARTH);
+}
+

src/lib/ecl/l1/ecl_l1_pos_controller.h

@@ -0,0 +1,268 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 Estimation and Control Library (ECL). 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 ECL 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 ecl_l1_pos_control.h
+ * Implementation of L1 position control.
+ *
+ *
+ * Acknowledgements and References:
+ *
+ *    This implementation has been built for PX4 based on the original
+ *    publication from [1] and does include a lot of the ideas (not code)
+ *    from [2].
+ *
+ *
+ *    [1] S. Park, J. Deyst, and J. P. How, "A New Nonlinear Guidance Logic for Trajectory Tracking,"
+ *    Proceedings of the AIAA Guidance, Navigation and Control
+ *    Conference, Aug 2004. AIAA-2004-4900.
+ *
+ *    [2] Paul Riseborough, Brandon Jones and Andrew Tridgell, L1 control for APM. Aug 2013.
+ *     - Explicit control over frequency and damping
+ *     - Explicit control over track capture angle
+ *     - Ability to use loiter radius smaller than L1 length
+ *     - Modified to use PD control for circle tracking to enable loiter radius less than L1 length
+ *     - Modified to enable period and damping of guidance loop to be set explicitly
+ *     - Modified to provide explicit control over capture angle
+ *
+ */
+
+#ifndef ECL_L1_POS_CONTROLLER_H
+#define ECL_L1_POS_CONTROLLER_H
+
+#include <mathlib/mathlib.h>
+#include <geo/geo.h>
+#include <ecl/ecl.h>
+
+/**
+ * L1 Nonlinear Guidance Logic
+ */
+class __EXPORT ECL_L1_Pos_Controller
+{
+public:
+	ECL_L1_Pos_Controller() {
+		_L1_period = 25;
+		_L1_damping = 0.75f;
+	}
+
+	/**
+	 * The current target bearing
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame)
+	 */
+	float nav_bearing();
+
+
+	/**
+	 * Get lateral acceleration demand.
+	 *
+	 * @return Lateral acceleration in m/s^2
+	 */
+	float nav_lateral_acceleration_demand();
+
+
+	/**
+	 * Heading error.
+	 *
+	 * The heading error is either compared to the current track
+	 * or to the tangent of the current loiter radius.
+	 */
+	float bearing_error();
+
+
+	/**
+	 * Bearing from aircraft to current target.
+	 *
+	 * @return bearing angle (-pi..pi, in NED frame)
+	 */
+	float target_bearing();
+
+
+	/**
+	 * Get roll angle setpoint for fixed wing.
+	 *
+	 * @return Roll angle (in NED frame)
+	 */
+	float nav_roll();
+
+
+	/**
+	 * Get the current crosstrack error.
+	 *
+	 * @return Crosstrack error in meters.
+	 */
+	float crosstrack_error();
+
+
+	/**
+	 * Returns true if the loiter waypoint has been reached
+	 */
+	bool reached_loiter_target();
+
+
+	/**
+	 * Returns true if following a circle (loiter)
+	 */
+	bool circle_mode() {
+		return _circle_mode;
+	}
+
+
+	/**
+	 * Get the switch distance
+	 * 
+	 * This is the distance at which the system will
+	 * switch to the next waypoint. This depends on the
+	 * period and damping
+	 *
+	 * @param waypoint_switch_radius The switching radius the waypoint has set.
+	 */
+	float switch_distance(float waypoint_switch_radius);
+
+
+	/**
+	 * Navigate between two waypoints
+	 *
+	 * Calling this function with two waypoints results in the
+	 * control outputs to fly to the line segment defined by
+	 * the points and once captured following the line segment.
+	 * This follows the logic in [1].
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_waypoints(const math::Vector2f &vector_A, const math::Vector2f &vector_B, const math::Vector2f &vector_curr_position,
+			   const math::Vector2f &ground_speed);
+
+
+	/**
+	 * Navigate on an orbit around a loiter waypoint.
+	 *
+	 * This allow orbits smaller than the L1 length,
+	 * this modification was introduced in [2].
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_loiter(const math::Vector2f &vector_A, const math::Vector2f &vector_curr_position, float radius, int8_t loiter_direction,
+			   const math::Vector2f &ground_speed_vector);
+
+
+	/**
+	 * Navigate on a fixed bearing.
+	 *
+	 * This only holds a certain direction and does not perform cross
+	 * track correction. Helpful for semi-autonomous modes. Introduced
+	 * by [2].
+	 *
+	 * @return sets _lateral_accel setpoint
+	 */
+	void navigate_heading(float navigation_heading, float current_heading, const math::Vector2f &ground_speed);
+
+
+	/**
+	 * Keep the wings level.
+	 *
+	 * This is typically needed for maximum-lift-demand situations,
+	 * such as takeoff or near stall. Introduced in [2].
+	 */
+	void navigate_level_flight(float current_heading);
+
+
+	/**
+	 * Set the L1 period.
+	 */
+	void set_l1_period(float period) {
+		_L1_period = period;
+		/* calculate the ratio introduced in [2] */
+		_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
+		/* calculate normalized frequency for heading tracking */
+		_heading_omega = sqrtf(2.0f) * M_PI_F / _L1_period;
+	}
+
+
+	/**
+	 * Set the L1 damping factor.
+	 *
+	 * The original publication recommends a default of sqrt(2) / 2 = 0.707
+	 */
+	void set_l1_damping(float damping) {
+		_L1_damping = damping;
+		/* calculate the ratio introduced in [2] */
+		_L1_ratio = 1.0f / M_PI_F * _L1_damping * _L1_period;
+		/* calculate the L1 gain (following [2]) */
+		_K_L1 = 4.0f * _L1_damping * _L1_damping;
+	}
+
+
+	/**
+	 * Set the maximum roll angle output in radians
+	 *
+	 */
+	void set_l1_roll_limit(float roll_lim_rad) {
+		_roll_lim_rad = roll_lim_rad;
+	}
+
+private:
+
+	float _lateral_accel;		///< Lateral acceleration setpoint in m/s^2
+	float _L1_distance;		///< L1 lead distance, defined by period and damping
+	bool _circle_mode;		///< flag for loiter mode
+	float _nav_bearing;		///< bearing to L1 reference point
+	float _bearing_error;		///< bearing error
+	float _crosstrack_error;	///< crosstrack error in meters
+	float _target_bearing;		///< the heading setpoint
+
+	float _L1_period;		///< L1 tracking period in seconds
+	float _L1_damping;		///< L1 damping ratio
+	float _L1_ratio;		///< L1 ratio for navigation
+	float _K_L1;			///< L1 control gain for _L1_damping
+	float _heading_omega;		///< Normalized frequency
+
+	float _roll_lim_rad;  ///<maximum roll angle
+
+	/**
+	 * Convert a 2D vector from WGS84 to planar coordinates.
+	 *
+	 * This converts from latitude and longitude to planar
+	 * coordinates with (0,0) being at the position of ref and
+	 * returns a vector in meters towards wp.
+	 *
+	 * @param ref The reference position in WGS84 coordinates
+	 * @param wp The point to convert to into the local coordinates, in WGS84 coordinates
+	 * @return The vector in meters pointing from the reference position to the coordinates
+	 */
+	math::Vector2f get_local_planar_vector(const math::Vector2f &origin, const math::Vector2f &target) const;
+
+};
+
+
+#endif /* ECL_L1_POS_CONTROLLER_H */

src/lib/ecl/module.mk

@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2013 Estimation and Control Library (ECL). 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.
+#
+############################################################################
+
+#
+# Estimation and Control Library
+#
+
+SRCS		 = 	attitude_fw/ecl_pitch_controller.cpp \
+			attitude_fw/ecl_roll_controller.cpp \
+			attitude_fw/ecl_yaw_controller.cpp \
+			l1/ecl_l1_pos_controller.cpp

src/lib/external_lgpl/module.mk

@@ -0,0 +1,48 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+#  W A R N I N G: The contents of this directory are license-incompatible
+# 			with the rest of the codebase. Do NOT copy any parts of it
+#			into other folders.
+#
+#  Acknowledgements:
+#
+#			The algorithms in this folder have been developed by Paul Riseborough
+#			with support from Andrew Tridgell.
+#			Originally licensed as LGPL for APM. As this is built as library and
+#			linked, use of this code does not change the usability of PX4 in general
+#			or any of the license implications.
+#
+
+SRCS		 = 	tecs/tecs.cpp

src/lib/external_lgpl/tecs/tecs.cpp

@@ -0,0 +1,534 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+#include "tecs.h"
+#include <ecl/ecl.h>
+
+using namespace math;
+
+#ifndef CONSTANTS_ONE_G
+#define CONSTANTS_ONE_G GRAVITY
+#endif
+
+/**
+ * @file tecs.cpp
+ *
+ * @author Paul Riseborough
+ *
+ *  Written by Paul Riseborough 2013 to provide:
+ *  - Combined control of speed and height using throttle to control
+ *    total energy and pitch angle to control exchange of energy between
+ *    potential and kinetic.
+ *    Selectable speed or height priority modes when calculating pitch angle
+ *  - Fallback mode when no airspeed measurement is available that
+ *    sets throttle based on height rate demand and switches pitch angle control to
+ *    height priority
+ *  - Underspeed protection that demands maximum throttle and switches pitch angle control
+ *    to speed priority mode
+ *  - Relative ease of tuning through use of intuitive time constant, integrator and damping gains and the use
+ *    of easy to measure aircraft performance data
+ *
+ */
+
+void TECS::update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth)
+{
+	// Implement third order complementary filter for height and height rate
+	// estimted height rate = _integ2_state
+	// estimated height     = _integ3_state
+	// Reference Paper :
+	// Optimising the Gains of the Baro-Inertial Vertical Channel
+	// Widnall W.S, Sinha P.K,
+	// AIAA Journal of Guidance and Control, 78-1307R
+
+	// Calculate time in seconds since last update
+	uint64_t now = ecl_absolute_time();
+	float DT = max((now - _update_50hz_last_usec), 0ULL) * 1.0e-6f;
+
+	// printf("dt: %10.6f baro alt: %6.2f eas: %6.2f R(0,0): %6.2f, R(1,1): %6.2f\naccel body: %6.2f %6.2f %6.2f\naccel earth: %6.2f %6.2f %6.2f\n",
+	// 	DT, baro_altitude, airspeed, rotMat(0, 0), rotMat(1, 1), accel_body(0), accel_body(1), accel_body(2),
+	// 	accel_earth(0), accel_earth(1), accel_earth(2));
+
+	if (DT > 1.0f) {
+		_integ3_state = baro_altitude;
+		_integ2_state = 0.0f;
+		_integ1_state = 0.0f;
+		DT            = 0.02f; // when first starting TECS, use a
+		// small time constant
+	}
+
+	_update_50hz_last_usec = now;
+	_EAS = airspeed;
+
+	// Get height acceleration
+	float hgt_ddot_mea = -(accel_earth(2) + CONSTANTS_ONE_G);
+	// Perform filter calculation using backwards Euler integration
+	// Coefficients selected to place all three filter poles at omega
+	float omega2 = _hgtCompFiltOmega * _hgtCompFiltOmega;
+	float hgt_err = baro_altitude - _integ3_state;
+	float integ1_input = hgt_err * omega2 * _hgtCompFiltOmega;
+	_integ1_state = _integ1_state + integ1_input * DT;
+	float integ2_input = _integ1_state + hgt_ddot_mea + hgt_err * omega2 * 3.0f;
+	_integ2_state = _integ2_state + integ2_input * DT;
+	float integ3_input = _integ2_state + hgt_err * _hgtCompFiltOmega * 3.0f;
+
+	// If more than 1 second has elapsed since last update then reset the integrator state
+	// to the measured height
+	if (DT > 1.0f) {
+		_integ3_state = baro_altitude;
+
+	} else {
+		_integ3_state = _integ3_state + integ3_input * DT;
+	}
+
+	// Update and average speed rate of change
+	// Only required if airspeed is being measured and controlled
+	float temp = 0;
+
+	if (isfinite(airspeed) && airspeed_sensor_enabled()) {
+		// Get DCM
+		// Calculate speed rate of change
+		// XXX check
+		temp = rotMat(2, 0) * CONSTANTS_ONE_G + accel_body(0);
+		// take 5 point moving average
+		//_vel_dot = _vdot_filter.apply(temp);
+		// XXX resolve this properly
+		_vel_dot = 0.9f * _vel_dot + 0.1f * temp;
+
+	} else {
+		_vel_dot = 0.0f;
+	}
+
+}
+
+void TECS::_update_speed(float airspeed_demand, float indicated_airspeed,
+			 float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS)
+{
+	// Calculate time in seconds since last update
+	uint64_t now = ecl_absolute_time();
+	float DT = max((now - _update_speed_last_usec), 0ULL) * 1.0e-6f;
+	_update_speed_last_usec = now;
+
+	// Convert equivalent airspeeds to true airspeeds
+
+	_EAS_dem = airspeed_demand;
+	_TAS_dem  = _EAS_dem * EAS2TAS;
+	_TASmax   = indicated_airspeed_max * EAS2TAS;
+	_TASmin   = indicated_airspeed_min * EAS2TAS;
+
+	// Reset states of time since last update is too large
+	if (DT > 1.0f) {
+		_integ5_state = (_EAS * EAS2TAS);
+		_integ4_state = 0.0f;
+		DT            = 0.1f; // when first starting TECS, use a
+		// small time constant
+	}
+
+	// Get airspeed or default to halfway between min and max if
+	// airspeed is not being used and set speed rate to zero
+	if (!isfinite(indicated_airspeed) || !airspeed_sensor_enabled()) {
+		// If no airspeed available use average of min and max
+		_EAS = 0.5f * (indicated_airspeed_min + indicated_airspeed_max);
+
+	} else {
+		_EAS = indicated_airspeed;
+	}
+
+	// Implement a second order complementary filter to obtain a
+	// smoothed airspeed estimate
+	// airspeed estimate is held in _integ5_state
+	float aspdErr = (_EAS * EAS2TAS) - _integ5_state;
+	float integ4_input = aspdErr * _spdCompFiltOmega * _spdCompFiltOmega;
+
+	// Prevent state from winding up
+	if (_integ5_state < 3.1f) {
+		integ4_input = max(integ4_input , 0.0f);
+	}
+
+	_integ4_state = _integ4_state + integ4_input * DT;
+	float integ5_input = _integ4_state + _vel_dot + aspdErr * _spdCompFiltOmega * 1.4142f;
+	_integ5_state = _integ5_state + integ5_input * DT;
+	// limit the airspeed to a minimum of 3 m/s
+	_integ5_state = max(_integ5_state, 3.0f);
+
+}
+
+void TECS::_update_speed_demand(void)
+{
+	// Set the airspeed demand to the minimum value if an underspeed condition exists
+	// or a bad descent condition exists
+	// This will minimise the rate of descent resulting from an engine failure,
+	// enable the maximum climb rate to be achieved and prevent continued full power descent
+	// into the ground due to an unachievable airspeed value
+	if ((_badDescent) || (_underspeed)) {
+		_TAS_dem     = _TASmin;
+	}
+
+	// Constrain speed demand
+	_TAS_dem = constrain(_TAS_dem, _TASmin, _TASmax);
+
+	// calculate velocity rate limits based on physical performance limits
+	// provision to use a different rate limit if bad descent or underspeed condition exists
+	// Use 50% of maximum energy rate to allow margin for total energy contgroller
+	float velRateMax;
+	float velRateMin;
+
+	if ((_badDescent) || (_underspeed)) {
+		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+
+	} else {
+		velRateMax = 0.5f * _STEdot_max / _integ5_state;
+		velRateMin = 0.5f * _STEdot_min / _integ5_state;
+	}
+
+	// Apply rate limit
+	if ((_TAS_dem - _TAS_dem_adj) > (velRateMax * 0.1f)) {
+		_TAS_dem_adj = _TAS_dem_adj + velRateMax * 0.1f;
+		_TAS_rate_dem = velRateMax;
+
+	} else if ((_TAS_dem - _TAS_dem_adj) < (velRateMin * 0.1f)) {
+		_TAS_dem_adj = _TAS_dem_adj + velRateMin * 0.1f;
+		_TAS_rate_dem = velRateMin;
+
+	} else {
+		_TAS_dem_adj = _TAS_dem;
+		_TAS_rate_dem = (_TAS_dem - _TAS_dem_last) / 0.1f;
+	}
+
+	// Constrain speed demand again to protect against bad values on initialisation.
+	_TAS_dem_adj = constrain(_TAS_dem_adj, _TASmin, _TASmax);
+	_TAS_dem_last = _TAS_dem;
+}
+
+void TECS::_update_height_demand(float demand)
+{
+	// Apply 2 point moving average to demanded height
+	// This is required because height demand is only updated at 5Hz
+	_hgt_dem = 0.5f * (demand + _hgt_dem_in_old);
+	_hgt_dem_in_old = _hgt_dem;
+
+	// printf("hgt_dem: %6.2f hgt_dem_last: %6.2f max_climb_rate: %6.2f\n", _hgt_dem, _hgt_dem_prev,
+	// 	_maxClimbRate);
+
+	// Limit height rate of change
+	if ((_hgt_dem - _hgt_dem_prev) > (_maxClimbRate * 0.1f)) {
+		_hgt_dem = _hgt_dem_prev + _maxClimbRate * 0.1f;
+
+	} else if ((_hgt_dem - _hgt_dem_prev) < (-_maxSinkRate * 0.1f)) {
+		_hgt_dem = _hgt_dem_prev - _maxSinkRate * 0.1f;
+	}
+
+	_hgt_dem_prev = _hgt_dem;
+
+	// Apply first order lag to height demand
+	_hgt_dem_adj = 0.05f * _hgt_dem + 0.95f * _hgt_dem_adj_last;
+	_hgt_rate_dem = (_hgt_dem_adj - _hgt_dem_adj_last) / 0.1f;
+	_hgt_dem_adj_last = _hgt_dem_adj;
+
+	// printf("hgt_dem: %6.2f hgt_dem_adj: %6.2f hgt_dem_last: %6.2f hgt_rate_dem: %6.2f\n", _hgt_dem, _hgt_dem_adj, _hgt_dem_adj_last,
+	// 	_hgt_rate_dem);
+}
+
+void TECS::_detect_underspeed(void)
+{
+	if (((_integ5_state < _TASmin * 0.9f) && (_throttle_dem >= _THRmaxf * 0.95f)) || ((_integ3_state < _hgt_dem_adj) && _underspeed)) {
+		_underspeed = true;
+
+	} else {
+		_underspeed = false;
+	}
+}
+
+void TECS::_update_energies(void)
+{
+	// Calculate specific energy demands
+	_SPE_dem = _hgt_dem_adj * CONSTANTS_ONE_G;
+	_SKE_dem = 0.5f * _TAS_dem_adj * _TAS_dem_adj;
+
+	// Calculate specific energy rate demands
+	_SPEdot_dem = _hgt_rate_dem * CONSTANTS_ONE_G;
+	_SKEdot_dem = _integ5_state * _TAS_rate_dem;
+
+	// Calculate specific energy
+	_SPE_est = _integ3_state * CONSTANTS_ONE_G;
+	_SKE_est = 0.5f * _integ5_state * _integ5_state;
+
+	// Calculate specific energy rate
+	_SPEdot = _integ2_state * CONSTANTS_ONE_G;
+	_SKEdot = _integ5_state * _vel_dot;
+}
+
+void TECS::_update_throttle(float throttle_cruise, const math::Dcm &rotMat)
+{
+	// Calculate total energy values
+	_STE_error = _SPE_dem - _SPE_est + _SKE_dem - _SKE_est;
+	float STEdot_dem = constrain((_SPEdot_dem + _SKEdot_dem), _STEdot_min, _STEdot_max);
+	float STEdot_error = STEdot_dem - _SPEdot - _SKEdot;
+
+	// Apply 0.5 second first order filter to STEdot_error
+	// This is required to remove accelerometer noise from the  measurement
+	STEdot_error = 0.2f * STEdot_error + 0.8f * _STEdotErrLast;
+	_STEdotErrLast = STEdot_error;
+
+	// Calculate throttle demand
+	// If underspeed condition is set, then demand full throttle
+	if (_underspeed) {
+		_throttle_dem_unc = 1.0f;
+
+	} else {
+		// Calculate gain scaler from specific energy error to throttle
+		float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min));
+
+		// Calculate feed-forward throttle
+		float ff_throttle = 0;
+		float nomThr = throttle_cruise;
+		// Use the demanded rate of change of total energy as the feed-forward demand, but add
+		// additional component which scales with (1/cos(bank angle) - 1) to compensate for induced
+		// drag increase during turns.
+		float cosPhi = sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1)));
+		STEdot_dem = STEdot_dem + _rollComp * (1.0f / constrain(cosPhi * cosPhi , 0.1f, 1.0f) - 1.0f);
+
+		if (STEdot_dem >= 0) {
+			ff_throttle = nomThr + STEdot_dem / _STEdot_max * (1.0f - nomThr);
+
+		} else {
+			ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
+		}
+
+		// Calculate PD + FF throttle
+		_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+
+		// Rate limit PD + FF throttle
+		// Calculate the throttle increment from the specified slew time
+		if (fabsf(_throttle_slewrate) < 0.01f) {
+			float thrRateIncr = _DT * (_THRmaxf - _THRminf) * _throttle_slewrate;
+
+			_throttle_dem = constrain(_throttle_dem,
+						  _last_throttle_dem - thrRateIncr,
+						  _last_throttle_dem + thrRateIncr);
+			_last_throttle_dem = _throttle_dem;
+		}
+
+
+		// Calculate integrator state upper and lower limits
+		// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand
+		float integ_max = (_THRmaxf - _throttle_dem + 0.1f);
+		float integ_min = (_THRminf - _throttle_dem - 0.1f);
+
+		// Calculate integrator state, constraining state
+		// Set integrator to a max throttle value dduring climbout
+		_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
+
+		if (_climbOutDem) {
+			_integ6_state = integ_max;
+
+		} else {
+			_integ6_state = constrain(_integ6_state, integ_min, integ_max);
+		}
+
+		// Sum the components.
+		// Only use feed-forward component if airspeed is not being used
+		if (airspeed_sensor_enabled()) {
+			_throttle_dem = _throttle_dem + _integ6_state;
+
+		} else {
+			_throttle_dem = ff_throttle;
+		}
+	}
+
+	// Constrain throttle demand
+	_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
+}
+
+void TECS::_detect_bad_descent(void)
+{
+	// Detect a demanded airspeed too high for the aircraft to achieve. This will be
+	// evident by the the following conditions:
+	// 1) Underspeed protection not active
+	// 2) Specific total energy error > 200 (greater than ~20m height error)
+	// 3) Specific total energy reducing
+	// 4) throttle demand > 90%
+	// If these four conditions exist simultaneously, then the protection
+	// mode will be activated.
+	// Once active, the following condition are required to stay in the mode
+	// 1) Underspeed protection not active
+	// 2) Specific total energy error > 0
+	// This mode will produce an undulating speed and height response as it cuts in and out but will prevent the aircraft from descending into the ground if an unachievable speed demand is set
+	float STEdot = _SPEdot + _SKEdot;
+
+	if ((!_underspeed && (_STE_error > 200.0f) && (STEdot < 0.0f) && (_throttle_dem >= _THRmaxf * 0.9f)) || (_badDescent && !_underspeed && (_STE_error > 0.0f))) {
+		_badDescent = true;
+
+	} else {
+		_badDescent = false;
+	}
+}
+
+void TECS::_update_pitch(void)
+{
+	// Calculate Speed/Height Control Weighting
+	// This is used to determine how the pitch control prioritises speed and height control
+	// A weighting of 1 provides equal priority (this is the normal mode of operation)
+	// A SKE_weighting of 0 provides 100% priority to height control. This is used when no airspeed measurement is available
+	// A SKE_weighting of 2 provides 100% priority to speed control. This is used when an underspeed condition is detected
+	// or during takeoff/climbout where a minimum pitch angle is set to ensure height is gained. In this instance, if airspeed
+	// rises above the demanded value, the pitch angle will be increased by the TECS controller.
+	float SKE_weighting = constrain(_spdWeight, 0.0f, 2.0f);
+
+	if ((_underspeed || _climbOutDem) && airspeed_sensor_enabled()) {
+		SKE_weighting = 2.0f;
+
+	} else if (!airspeed_sensor_enabled()) {
+		SKE_weighting = 0.0f;
+	}
+
+	float SPE_weighting = 2.0f - SKE_weighting;
+
+	// Calculate Specific Energy Balance demand, and error
+	float SEB_dem      = _SPE_dem * SPE_weighting - _SKE_dem * SKE_weighting;
+	float SEBdot_dem   = _SPEdot_dem * SPE_weighting - _SKEdot_dem * SKE_weighting;
+	float SEB_error    = SEB_dem - (_SPE_est * SPE_weighting - _SKE_est * SKE_weighting);
+	float SEBdot_error = SEBdot_dem - (_SPEdot * SPE_weighting - _SKEdot * SKE_weighting);
+
+	// Calculate integrator state, constraining input if pitch limits are exceeded
+	float integ7_input = SEB_error * _integGain;
+
+	if (_pitch_dem_unc > _PITCHmaxf) {
+		integ7_input = min(integ7_input, _PITCHmaxf - _pitch_dem_unc);
+
+	} else if (_pitch_dem_unc < _PITCHminf) {
+		integ7_input = max(integ7_input, _PITCHminf - _pitch_dem_unc);
+	}
+
+	_integ7_state = _integ7_state + integ7_input * _DT;
+
+	// Apply max and min values for integrator state that will allow for no more than
+	// 5deg of saturation. This allows for some pitch variation due to gusts before the
+	// integrator is clipped. Otherwise the effectiveness of the integrator will be reduced in turbulence
+	float gainInv = (_integ5_state * _timeConst * CONSTANTS_ONE_G);
+	float temp = SEB_error + SEBdot_error * _ptchDamp + SEBdot_dem * _timeConst;
+	_integ7_state = constrain(_integ7_state, (gainInv * (_PITCHminf - 0.0783f)) - temp, (gainInv * (_PITCHmaxf + 0.0783f)) - temp);
+
+	// Calculate pitch demand from specific energy balance signals
+	_pitch_dem_unc = (temp + _integ7_state) / gainInv;
+
+	// Constrain pitch demand
+	_pitch_dem = constrain(_pitch_dem_unc, _PITCHminf, _PITCHmaxf);
+
+	// Rate limit the pitch demand to comply with specified vertical
+	// acceleration limit
+	float ptchRateIncr = _DT * _vertAccLim / _integ5_state;
+
+	if ((_pitch_dem - _last_pitch_dem) > ptchRateIncr) {
+		_pitch_dem = _last_pitch_dem + ptchRateIncr;
+
+	} else if ((_pitch_dem - _last_pitch_dem) < -ptchRateIncr) {
+		_pitch_dem = _last_pitch_dem - ptchRateIncr;
+	}
+
+	_last_pitch_dem = _pitch_dem;
+}
+
+void TECS::_initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad)
+{
+	// Initialise states and variables if DT > 1 second or in climbout
+	if (_DT > 1.0f) {
+		_integ6_state      = 0.0f;
+		_integ7_state      = 0.0f;
+		_last_throttle_dem = throttle_cruise;
+		_last_pitch_dem    = pitch;
+		_hgt_dem_adj_last  = baro_altitude;
+		_hgt_dem_adj       = _hgt_dem_adj_last;
+		_hgt_dem_prev      = _hgt_dem_adj_last;
+		_hgt_dem_in_old    = _hgt_dem_adj_last;
+		_TAS_dem_last      = _TAS_dem;
+		_TAS_dem_adj       = _TAS_dem;
+		_underspeed        = false;
+		_badDescent        = false;
+		_DT                = 0.1f; // when first starting TECS, use a
+		// small time constant
+
+	} else if (_climbOutDem) {
+		_PITCHminf          = ptchMinCO_rad;
+		_THRminf            = _THRmaxf - 0.01f;
+		_hgt_dem_adj_last  = baro_altitude;
+		_hgt_dem_adj       = _hgt_dem_adj_last;
+		_hgt_dem_prev      = _hgt_dem_adj_last;
+		_TAS_dem_last      = _TAS_dem;
+		_TAS_dem_adj       = _TAS_dem;
+		_underspeed        = false;
+		_badDescent 	   = false;
+	}
+}
+
+void TECS::_update_STE_rate_lim(void)
+{
+	// Calculate Specific Total Energy Rate Limits
+	// This is a tivial calculation at the moment but will get bigger once we start adding altitude effects
+	_STEdot_max = _maxClimbRate * CONSTANTS_ONE_G;
+	_STEdot_min = - _minSinkRate * CONSTANTS_ONE_G;
+}
+
+void TECS::update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+				 float throttle_min, float throttle_max, float throttle_cruise,
+				 float pitch_limit_min, float pitch_limit_max)
+{
+	// Calculate time in seconds since last update
+	uint64_t now = ecl_absolute_time();
+	_DT = max((now - _update_pitch_throttle_last_usec), 0ULL) * 1.0e-6f;
+	_update_pitch_throttle_last_usec = now;
+
+	// printf("tecs in: dt:%10.6f pitch: %6.2f baro_alt: %6.2f alt sp: %6.2f\neas sp: %6.2f eas: %6.2f, eas2tas: %6.2f\n %s pitch min C0: %6.2f thr min: %6.2f, thr max: %6.2f thr cruis: %6.2f pt min: %6.2f, pt max: %6.2f\n",
+	// 	_DT, pitch, baro_altitude, hgt_dem, EAS_dem, indicated_airspeed, EAS2TAS, (climbOutDem) ? "climb" : "level", ptchMinCO, throttle_min, throttle_max, throttle_cruise, pitch_limit_min, pitch_limit_max);
+
+	// Update the speed estimate using a 2nd order complementary filter
+	_update_speed(EAS_dem, indicated_airspeed, _indicated_airspeed_min, _indicated_airspeed_max, EAS2TAS);
+
+	// Convert inputs
+	_THRmaxf  = throttle_max;
+	_THRminf  = throttle_min;
+	_PITCHmaxf = pitch_limit_max;
+	_PITCHminf = pitch_limit_min;
+	_climbOutDem = climbOutDem;
+
+	// initialise selected states and variables if DT > 1 second or in climbout
+	_initialise_states(pitch, throttle_cruise, baro_altitude, ptchMinCO);
+
+	// Calculate Specific Total Energy Rate Limits
+	_update_STE_rate_lim();
+
+	// Calculate the speed demand
+	_update_speed_demand();
+
+	// Calculate the height demand
+	_update_height_demand(hgt_dem);
+
+	// Detect underspeed condition
+	_detect_underspeed();
+
+	// Calculate specific energy quantitiues
+	_update_energies();
+
+	// Calculate throttle demand
+	_update_throttle(throttle_cruise, rotMat);
+
+	// Detect bad descent due to demanded airspeed being too high
+	_detect_bad_descent();
+
+	// Calculate pitch demand
+	_update_pitch();
+
+//    // Write internal variables to the log_tuning structure. This
+//    // structure will be logged in dataflash at 10Hz
+	// log_tuning.hgt_dem  = _hgt_dem_adj;
+	// log_tuning.hgt      = _integ3_state;
+	// log_tuning.dhgt_dem = _hgt_rate_dem;
+	// log_tuning.dhgt     = _integ2_state;
+	// log_tuning.spd_dem  = _TAS_dem_adj;
+	// log_tuning.spd      = _integ5_state;
+	// log_tuning.dspd     = _vel_dot;
+	// log_tuning.ithr     = _integ6_state;
+	// log_tuning.iptch    = _integ7_state;
+	// log_tuning.thr      = _throttle_dem;
+	// log_tuning.ptch     = _pitch_dem;
+	// log_tuning.dspd_dem = _TAS_rate_dem;
+}

src/lib/external_lgpl/tecs/tecs.h

@@ -0,0 +1,357 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: t -*-
+
+/// @file    tecs.h
+/// @brief   Combined Total Energy Speed & Height Control.
+
+/*
+ *  Written by Paul Riseborough 2013 to provide:
+ *  - Combined control of speed and height using throttle to control
+ *    total energy and pitch angle to control exchange of energy between
+ *    potential and kinetic.
+ *    Selectable speed or height priority modes when calculating pitch angle
+ *  - Fallback mode when no airspeed measurement is available that
+ *    sets throttle based on height rate demand and switches pitch angle control to
+ *    height priority
+ *  - Underspeed protection that demands maximum throttle switches pitch angle control
+ *    to speed priority mode
+ *  - Relative ease of tuning through use of intuitive time constant, trim rate and damping parameters and the use
+ *    of easy to measure aircraft performance data
+ */
+
+#ifndef TECS_H
+#define TECS_H
+
+#include <mathlib/mathlib.h>
+#include <stdint.h>
+
+class __EXPORT TECS
+{
+public:
+	TECS() :
+
+		_airspeed_enabled(false),
+		_throttle_slewrate(0.0f),
+		_climbOutDem(false),
+		_hgt_dem_prev(0.0f),
+		_hgt_dem_adj_last(0.0f),
+		_hgt_dem_in_old(0.0f),
+		_TAS_dem_last(0.0f),
+		_TAS_dem_adj(0.0f),
+		_TAS_dem(0.0f),
+		_integ1_state(0.0f),
+		_integ2_state(0.0f),
+		_integ3_state(0.0f),
+		_integ4_state(0.0f),
+		_integ5_state(0.0f),
+		_integ6_state(0.0f),
+		_integ7_state(0.0f),
+		_pitch_dem(0.0f),
+		_last_pitch_dem(0.0f),
+		_SPE_dem(0.0f),
+		_SKE_dem(0.0f),
+		_SPEdot_dem(0.0f),
+		_SKEdot_dem(0.0f),
+		_SPE_est(0.0f),
+		_SKE_est(0.0f),
+		_SPEdot(0.0f),
+		_SKEdot(0.0f),
+		_vel_dot(0.0f),
+		_STEdotErrLast(0.0f) {
+
+	}
+
+	bool airspeed_sensor_enabled() {
+		return _airspeed_enabled;
+	}
+
+	void enable_airspeed(bool enabled) {
+		_airspeed_enabled = enabled;
+	}
+
+	// Update of the estimated height and height rate internal state
+	// Update of the inertial speed rate internal state
+	// Should be called at 50Hz or greater
+	void update_50hz(float baro_altitude, float airspeed, const math::Dcm &rotMat, const math::Vector3 &accel_body, const math::Vector3 &accel_earth);
+
+	// Update the control loop calculations
+	void update_pitch_throttle(const math::Dcm &rotMat, float pitch, float baro_altitude, float hgt_dem, float EAS_dem, float indicated_airspeed, float EAS2TAS, bool climbOutDem, float ptchMinCO,
+				   float throttle_min, float throttle_max, float throttle_cruise,
+				   float pitch_limit_min, float pitch_limit_max);
+	// demanded throttle in percentage
+	// should return 0 to 100
+	float get_throttle_demand(void) {
+		return _throttle_dem;
+	}
+	int32_t get_throttle_demand_percent(void) {
+		return get_throttle_demand();
+	}
+
+
+	float get_pitch_demand() { return _pitch_dem; }
+
+	// demanded pitch angle in centi-degrees
+	// should return between -9000 to +9000
+	int32_t get_pitch_demand_cd() { return int32_t(get_pitch_demand() * 5729.5781f);}
+
+	// Rate of change of velocity along X body axis in m/s^2
+	float get_VXdot(void) { return _vel_dot; }
+
+
+	float get_speed_weight() {
+		return _spdWeight;
+	}
+
+	// log data on internal state of the controller. Called at 10Hz
+	// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
+
+	// struct PACKED log_TECS_Tuning {
+	// 	LOG_PACKET_HEADER;
+	// 	float hgt;
+	// 	float dhgt;
+	// 	float hgt_dem;
+	// 	float dhgt_dem;
+	// 	float spd_dem;
+	// 	float spd;
+	// 	float dspd;
+	// 	float ithr;
+	// 	float iptch;
+	// 	float thr;
+	// 	float ptch;
+	// 	float dspd_dem;
+	// } log_tuning;
+
+	void set_time_const(float time_const) {
+		_timeConst = time_const;
+	}
+
+	void set_min_sink_rate(float rate) {
+		_minSinkRate = rate;
+	}
+
+	void set_max_sink_rate(float sink_rate) {
+		_maxSinkRate = sink_rate;
+	}
+
+	void set_max_climb_rate(float climb_rate) {
+		_maxClimbRate = climb_rate;
+	}
+
+	void set_throttle_damp(float throttle_damp) {
+		_thrDamp = throttle_damp;
+	}
+
+	void set_integrator_gain(float gain) {
+		_integGain = gain;
+	}
+
+	void set_vertical_accel_limit(float limit) {
+		_vertAccLim = limit;
+	}
+
+	void set_height_comp_filter_omega(float omega) {
+		_hgtCompFiltOmega = omega;
+	}
+
+	void set_speed_comp_filter_omega(float omega) {
+		_spdCompFiltOmega = omega;
+	}
+
+	void set_roll_throttle_compensation(float compensation) {
+		_rollComp = compensation;
+	}
+
+	void set_speed_weight(float weight) {
+		_spdWeight = weight;
+	}
+
+	void set_pitch_damping(float damping) {
+		_ptchDamp = damping;
+	}
+
+	void set_throttle_slewrate(float slewrate) {
+		_throttle_slewrate = slewrate;
+	}
+
+	void set_indicated_airspeed_min(float airspeed) {
+		_indicated_airspeed_min = airspeed;
+	}
+
+	void set_indicated_airspeed_max(float airspeed) {
+		_indicated_airspeed_max = airspeed;
+	}
+
+private:
+	// Last time update_50Hz was called
+	uint64_t _update_50hz_last_usec;
+
+	// Last time update_speed was called
+	uint64_t _update_speed_last_usec;
+
+	// Last time update_pitch_throttle was called
+	uint64_t _update_pitch_throttle_last_usec;
+
+	// TECS tuning parameters
+	float _hgtCompFiltOmega;
+	float _spdCompFiltOmega;
+	float _maxClimbRate;
+	float _minSinkRate;
+	float _maxSinkRate;
+	float _timeConst;
+	float _ptchDamp;
+	float _thrDamp;
+	float _integGain;
+	float _vertAccLim;
+	float _rollComp;
+	float _spdWeight;
+
+	// throttle demand in the range from 0.0 to 1.0
+	float _throttle_dem;
+
+	// pitch angle demand in radians
+	float _pitch_dem;
+
+	// Integrator state 1 - height filter second derivative
+	float _integ1_state;
+
+	// Integrator state 2 - height rate
+	float _integ2_state;
+
+	// Integrator state 3 - height
+	float _integ3_state;
+
+	// Integrator state 4 - airspeed filter first derivative
+	float _integ4_state;
+
+	// Integrator state 5 - true airspeed
+	float _integ5_state;
+
+	// Integrator state 6 - throttle integrator
+	float _integ6_state;
+
+	// Integrator state 6 - pitch integrator
+	float _integ7_state;
+
+	// throttle demand rate limiter state
+	float _last_throttle_dem;
+
+	// pitch demand rate limiter state
+	float _last_pitch_dem;
+
+	// Rate of change of speed along X axis
+	float _vel_dot;
+
+	// Equivalent airspeed
+	float _EAS;
+
+	// True airspeed limits
+	float _TASmax;
+	float _TASmin;
+
+	// Current and last true airspeed demand
+	float _TAS_dem;
+	float _TAS_dem_last;
+
+	// Equivalent airspeed demand
+	float _EAS_dem;
+
+	// height demands
+	float _hgt_dem;
+	float _hgt_dem_in_old;
+	float _hgt_dem_adj;
+	float _hgt_dem_adj_last;
+	float _hgt_rate_dem;
+	float _hgt_dem_prev;
+
+	// Speed demand after application of rate limiting
+	// This is the demand tracked by the TECS control loops
+	float _TAS_dem_adj;
+
+	// Speed rate demand after application of rate limiting
+	// This is the demand tracked by the TECS control loops
+	float _TAS_rate_dem;
+
+	// Total energy rate filter state
+	float _STEdotErrLast;
+
+	// Underspeed condition
+	bool _underspeed;
+
+	// Bad descent condition caused by unachievable airspeed demand
+	bool _badDescent;
+
+	// climbout mode
+	bool _climbOutDem;
+
+	// throttle demand before limiting
+	float _throttle_dem_unc;
+
+	// pitch demand before limiting
+	float _pitch_dem_unc;
+
+	// Maximum and minimum specific total energy rate limits
+	float _STEdot_max;
+	float _STEdot_min;
+
+	// Maximum and minimum floating point throttle limits
+	float _THRmaxf;
+	float _THRminf;
+
+	// Maximum and minimum floating point pitch limits
+	float _PITCHmaxf;
+	float _PITCHminf;
+
+	// Specific energy quantities
+	float _SPE_dem;
+	float _SKE_dem;
+	float _SPEdot_dem;
+	float _SKEdot_dem;
+	float _SPE_est;
+	float _SKE_est;
+	float _SPEdot;
+	float _SKEdot;
+
+	// Specific energy error quantities
+	float _STE_error;
+
+	// Time since last update of main TECS loop (seconds)
+	float _DT;
+
+	bool _airspeed_enabled;
+	float _throttle_slewrate;
+	float _indicated_airspeed_min;
+	float _indicated_airspeed_max;
+
+	// Update the airspeed internal state using a second order complementary filter
+	void _update_speed(float airspeed_demand, float indicated_airspeed,
+			   float indicated_airspeed_min, float indicated_airspeed_max, float EAS2TAS);
+
+	// Update the demanded airspeed
+	void _update_speed_demand(void);
+
+	// Update the demanded height
+	void _update_height_demand(float demand);
+
+	// Detect an underspeed condition
+	void _detect_underspeed(void);
+
+	// Update Specific Energy Quantities
+	void _update_energies(void);
+
+	// Update Demanded Throttle
+	void _update_throttle(float throttle_cruise, const math::Dcm &rotMat);
+
+	// Detect Bad Descent
+	void _detect_bad_descent(void);
+
+	// Update Demanded Pitch Angle
+	void _update_pitch(void);
+
+	// Initialise states and variables
+	void _initialise_states(float pitch, float throttle_cruise, float baro_altitude, float ptchMinCO_rad);
+
+	// Calculate specific total energy rate limits
+	void _update_STE_rate_lim(void);
+
+};
+
+#endif //TECS_H

src/lib/geo/geo.c

@@ -0,0 +1,468 @@
+/****************************************************************************
+ *
+ *   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 <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;
+}
+
+__EXPORT void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+{
+	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 */
+	*vy = CONSTANTS_RADIUS_OF_EARTH * sin(d_lon) * cos(lat_next_rad);
+	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad) * sin(lat_next_rad) - sin(lat_now_rad) * cos(lat_next_rad) * cos(d_lon);
+}
+
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy)
+{
+	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 */
+	*vy = CONSTANTS_RADIUS_OF_EARTH * d_lon;
+	*vx = CONSTANTS_RADIUS_OF_EARTH * cos(lat_now_rad);
+}
+
+// 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;
+}
+
+

src/lib/geo/geo.h

@@ -0,0 +1,129 @@
+/****************************************************************************
+ *
+ *   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)		*/
+
+// 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 void get_vector_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+
+__EXPORT void get_vector_to_next_waypoint_fast(double lat_now, double lon_now, double lat_next, double lon_next, float* vx, float* vy);
+
+__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

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

src/lib/mathlib/CMSIS/Device/ARM/ARMCM3/Include/ARMCM3.h

@@ -0,0 +1,264 @@
+/**************************************************************************//**
+ * @file     ARMCM3.h
+ * @brief    CMSIS Core Peripheral Access Layer Header File for
+ *           ARMCM3 Device Series
+ * @version  V1.07
+ * @date     30. January 2012
+ *
+ * @note
+ * Copyright (C) 2012 ARM Limited. All rights reserved.
+ *
+ * @par
+ * ARM Limited (ARM) is supplying this software for use with Cortex-M 
+ * processor based microcontrollers.  This file can be freely distributed 
+ * within development tools that are supporting such ARM based processors. 
+ *
+ * @par
+ * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
+ * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
+ * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
+ * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
+ *
+ ******************************************************************************/
+
+#ifndef ARMCM3_H
+#define ARMCM3_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* -------------------------  Interrupt Number Definition  ------------------------ */
+
+typedef enum IRQn
+{
+/* -------------------  Cortex-M3 Processor Exceptions Numbers  ------------------- */
+  NonMaskableInt_IRQn           = -14,      /*!<  2 Non Maskable Interrupt          */
+  HardFault_IRQn                = -13,      /*!<  3 HardFault Interrupt             */
+  MemoryManagement_IRQn         = -12,      /*!<  4 Memory Management Interrupt     */
+  BusFault_IRQn                 = -11,      /*!<  5 Bus Fault Interrupt             */
+  UsageFault_IRQn               = -10,      /*!<  6 Usage Fault Interrupt           */
+  SVCall_IRQn                   =  -5,      /*!< 11 SV Call Interrupt               */
+  DebugMonitor_IRQn             =  -4,      /*!< 12 Debug Monitor Interrupt         */
+  PendSV_IRQn                   =  -2,      /*!< 14 Pend SV Interrupt               */
+  SysTick_IRQn                  =  -1,      /*!< 15 System Tick Interrupt           */
+
+/* ----------------------  ARMCM3 Specific Interrupt Numbers  --------------------- */
+  WDT_IRQn                      =   0,      /*!< Watchdog Timer Interrupt           */
+  RTC_IRQn                      =   1,      /*!< Real Time Clock Interrupt          */
+  TIM0_IRQn                     =   2,      /*!< Timer0 / Timer1 Interrupt          */
+  TIM2_IRQn                     =   3,      /*!< Timer2 / Timer3 Interrupt          */
+  MCIA_IRQn                     =   4,      /*!< MCIa Interrupt                     */
+  MCIB_IRQn                     =   5,      /*!< MCIb Interrupt                     */
+  UART0_IRQn                    =   6,      /*!< UART0 Interrupt                    */
+  UART1_IRQn                    =   7,      /*!< UART1 Interrupt                    */
+  UART2_IRQn                    =   8,      /*!< UART2 Interrupt                    */
+  UART4_IRQn                    =   9,      /*!< UART4 Interrupt                    */
+  AACI_IRQn                     =  10,      /*!< AACI / AC97 Interrupt              */
+  CLCD_IRQn                     =  11,      /*!< CLCD Combined Interrupt            */
+  ENET_IRQn                     =  12,      /*!< Ethernet Interrupt                 */
+  USBDC_IRQn                    =  13,      /*!< USB Device Interrupt               */
+  USBHC_IRQn                    =  14,      /*!< USB Host Controller Interrupt      */
+  CHLCD_IRQn                    =  15,      /*!< Character LCD Interrupt            */
+  FLEXRAY_IRQn                  =  16,      /*!< Flexray Interrupt                  */
+  CAN_IRQn                      =  17,      /*!< CAN Interrupt                      */
+  LIN_IRQn                      =  18,      /*!< LIN Interrupt                      */
+  I2C_IRQn                      =  19,      /*!< I2C ADC/DAC Interrupt              */
+  CPU_CLCD_IRQn                 =  28,      /*!< CPU CLCD Combined Interrupt        */
+  UART3_IRQn                    =  30,      /*!< UART3 Interrupt                    */
+  SPI_IRQn                      =  31,      /*!< SPI Touchscreen Interrupt          */
+} IRQn_Type;
+
+
+/* ================================================================================ */
+/* ================      Processor and Core Peripheral Section     ================ */
+/* ================================================================================ */
+
+/* --------  Configuration of the Cortex-M4 Processor and Core Peripherals  ------- */
+#define __CM3_REV                 0x0201    /*!< Core revision r2p1                              */
+#define __MPU_PRESENT             1         /*!< MPU present or not                              */
+#define __NVIC_PRIO_BITS          3         /*!< Number of Bits used for Priority Levels         */
+#define __Vendor_SysTickConfig    0         /*!< Set to 1 if different SysTick Config is used    */
+
+#include <core_cm3.h>                       /* Processor and core peripherals                    */
+/* NuttX */
+//#include "system_ARMCM3.h"                  /* System Header                                     */
+
+
+/* ================================================================================ */
+/* ================       Device Specific Peripheral Section       ================ */
+/* ================================================================================ */
+
+/* -------------------  Start of section using anonymous unions  ------------------ */
+#if defined(__CC_ARM)
+  #pragma push
+  #pragma anon_unions
+#elif defined(__ICCARM__)
+  #pragma language=extended
+#elif defined(__GNUC__)
+  /* anonymous unions are enabled by default */
+#elif defined(__TMS470__)
+/* anonymous unions are enabled by default */
+#elif defined(__TASKING__)
+  #pragma warning 586
+#else
+  #warning Not supported compiler type
+#endif
+
+
+
+/* ================================================================================ */
+/* ================            CPU FPGA System (CPU_SYS)           ================ */
+/* ================================================================================ */
+typedef struct
+{
+  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */
+  __IO uint32_t MEMCFG;           /* Offset: 0x004 (R/W)  Remap and Alias Memory Control */
+  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */
+  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */
+  __I  uint32_t TS;               /* Offset: 0x010 (R/ )  Touchscreen Register */
+  __IO uint32_t CTRL1;            /* Offset: 0x014 (R/W)  Misc Control Functions */
+       uint32_t RESERVED0[2];
+  __IO uint32_t CLKCFG;           /* Offset: 0x020 (R/W)  System Clock Configuration */
+  __IO uint32_t WSCFG;            /* Offset: 0x024 (R/W)  Flash Waitstate Configuration */
+  __IO uint32_t CPUCFG;           /* Offset: 0x028 (R/W)  Processor Configuration */
+       uint32_t RESERVED1[3];
+  __IO uint32_t BASE;             /* Offset: 0x038 (R/W)  ROM Table base Address */
+  __IO uint32_t ID2;              /* Offset: 0x03C (R/W)  Secondary Identification Register */
+} ARM_CPU_SYS_TypeDef;
+
+
+/* ================================================================================ */
+/* ================            DUT FPGA System (DUT_SYS)           ================ */
+/* ================================================================================ */
+typedef struct
+{
+  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */
+  __IO uint32_t PERCFG;           /* Offset: 0x004 (R/W)  Peripheral Control Signals */
+  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */
+  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */
+  __IO uint32_t SEG7;             /* Offset: 0x010 (R/W)  7-segment LED Output States */
+  __I  uint32_t CNT25MHz;         /* Offset: 0x014 (R/ )  Freerunning counter incrementing at 25MHz */
+  __I  uint32_t CNT100Hz;         /* Offset: 0x018 (R/ )  Freerunning counter incrementing at 100Hz */
+} ARM_DUT_SYS_TypeDef;
+
+
+/* ================================================================================ */
+/* ================                   Timer (TIM)                  ================ */
+/* ================================================================================ */
+typedef struct
+{
+  __IO uint32_t Timer1Load;       /* Offset: 0x000 (R/W)  Timer 1 Load */
+  __I  uint32_t Timer1Value;      /* Offset: 0x004 (R/ )  Timer 1 Counter Current Value */
+  __IO uint32_t Timer1Control;    /* Offset: 0x008 (R/W)  Timer 1 Control */
+  __O  uint32_t Timer1IntClr;     /* Offset: 0x00C ( /W)  Timer 1 Interrupt Clear */
+  __I  uint32_t Timer1RIS;        /* Offset: 0x010 (R/ )  Timer 1 Raw Interrupt Status */
+  __I  uint32_t Timer1MIS;        /* Offset: 0x014 (R/ )  Timer 1 Masked Interrupt Status */
+  __IO uint32_t Timer1BGLoad;     /* Offset: 0x018 (R/W)  Background Load Register */
+       uint32_t RESERVED0[1];
+  __IO uint32_t Timer2Load;       /* Offset: 0x020 (R/W)  Timer 2 Load */
+  __I  uint32_t Timer2Value;      /* Offset: 0x024 (R/ )  Timer 2 Counter Current Value */
+  __IO uint32_t Timer2Control;    /* Offset: 0x028 (R/W)  Timer 2 Control */
+  __O  uint32_t Timer2IntClr;     /* Offset: 0x02C ( /W)  Timer 2 Interrupt Clear */
+  __I  uint32_t Timer2RIS;        /* Offset: 0x030 (R/ )  Timer 2 Raw Interrupt Status */
+  __I  uint32_t Timer2MIS;        /* Offset: 0x034 (R/ )  Timer 2 Masked Interrupt Status */
+  __IO uint32_t Timer2BGLoad;     /* Offset: 0x038 (R/W)  Background Load Register */
+} ARM_TIM_TypeDef;
+
+
+/* ================================================================================ */
+/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */
+/* ================================================================================ */
+typedef struct
+{
+  __IO uint32_t DR;               /* Offset: 0x000 (R/W)  Data */ 
+  union {
+  __I  uint32_t RSR;              /* Offset: 0x000 (R/ )  Receive Status */
+  __O  uint32_t ECR;              /* Offset: 0x000 ( /W)  Error Clear */
+  };
+       uint32_t RESERVED0[4];
+  __IO uint32_t FR;               /* Offset: 0x018 (R/W)  Flags */
+       uint32_t RESERVED1[1];
+  __IO uint32_t ILPR;             /* Offset: 0x020 (R/W)  IrDA Low-power Counter */
+  __IO uint32_t IBRD;             /* Offset: 0x024 (R/W)  Interger Baud Rate */
+  __IO uint32_t FBRD;             /* Offset: 0x028 (R/W)  Fractional Baud Rate */
+  __IO uint32_t LCR_H;            /* Offset: 0x02C (R/W)  Line Control */
+  __IO uint32_t CR;               /* Offset: 0x030 (R/W)  Control */
+  __IO uint32_t IFLS;             /* Offset: 0x034 (R/W)  Interrupt FIFO Level Select */
+  __IO uint32_t IMSC;             /* Offset: 0x038 (R/W)  Interrupt Mask Set / Clear */
+  __IO uint32_t RIS;              /* Offset: 0x03C (R/W)  Raw Interrupt Status */
+  __IO uint32_t MIS;              /* Offset: 0x040 (R/W)  Masked Interrupt Status */
+  __O  uint32_t ICR;              /* Offset: 0x044 ( /W)  Interrupt Clear */
+  __IO uint32_t DMACR;            /* Offset: 0x048 (R/W)  DMA Control */
+} ARM_UART_TypeDef;
+
+
+/* --------------------  End of section using anonymous unions  ------------------- */
+#if defined(__CC_ARM)
+  #pragma pop
+#elif defined(__ICCARM__)
+  /* leave anonymous unions enabled */
+#elif defined(__GNUC__)
+  /* anonymous unions are enabled by default */
+#elif defined(__TMS470__)
+  /* anonymous unions are enabled by default */
+#elif defined(__TASKING__)
+  #pragma warning restore
+#else
+  #warning Not supported compiler type
+#endif
+
+
+
+
+/* ================================================================================ */
+/* ================              Peripheral memory map             ================ */
+/* ================================================================================ */
+/* --------------------------  CPU FPGA memory map  ------------------------------- */
+#define ARM_FLASH_BASE            (0x00000000UL)
+#define ARM_RAM_BASE              (0x20000000UL)
+#define ARM_RAM_FPGA_BASE         (0x1EFF0000UL)
+#define ARM_CPU_CFG_BASE          (0xDFFF0000UL)
+
+#define ARM_CPU_SYS_BASE          (ARM_CPU_CFG_BASE  + 0x00000)
+#define ARM_UART3_BASE            (ARM_CPU_CFG_BASE  + 0x05000)
+
+/* --------------------------  DUT FPGA memory map  ------------------------------- */
+#define ARM_APB_BASE              (0x40000000UL)
+#define ARM_AHB_BASE              (0x4FF00000UL)
+#define ARM_DMC_BASE              (0x60000000UL)
+#define ARM_SMC_BASE              (0xA0000000UL)
+
+#define ARM_TIM0_BASE             (ARM_APB_BASE      + 0x02000)
+#define ARM_TIM2_BASE             (ARM_APB_BASE      + 0x03000)
+#define ARM_DUT_SYS_BASE          (ARM_APB_BASE      + 0x04000)
+#define ARM_UART0_BASE            (ARM_APB_BASE      + 0x06000)
+#define ARM_UART1_BASE            (ARM_APB_BASE      + 0x07000)
+#define ARM_UART2_BASE            (ARM_APB_BASE      + 0x08000)
+#define ARM_UART4_BASE            (ARM_APB_BASE      + 0x09000)
+
+
+/* ================================================================================ */
+/* ================             Peripheral declaration             ================ */
+/* ================================================================================ */
+/* --------------------------  CPU FPGA Peripherals  ------------------------------ */
+#define ARM_CPU_SYS               ((ARM_CPU_SYS_TypeDef *)  ARM_CPU_SYS_BASE)
+#define ARM_UART3                 ((   ARM_UART_TypeDef *)    ARM_UART3_BASE)
+
+/* --------------------------  DUT FPGA Peripherals  ------------------------------ */
+#define ARM_DUT_SYS               ((ARM_DUT_SYS_TypeDef *)  ARM_DUT_SYS_BASE)
+#define ARM_TIM0                  ((    ARM_TIM_TypeDef *)     ARM_TIM0_BASE)
+#define ARM_TIM2                  ((    ARM_TIM_TypeDef *)     ARM_TIM2_BASE)
+#define ARM_UART0                 ((   ARM_UART_TypeDef *)    ARM_UART0_BASE)
+#define ARM_UART1                 ((   ARM_UART_TypeDef *)    ARM_UART1_BASE)
+#define ARM_UART2                 ((   ARM_UART_TypeDef *)    ARM_UART2_BASE)
+#define ARM_UART4                 ((   ARM_UART_TypeDef *)    ARM_UART4_BASE)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* ARMCM3_H */

src/lib/mathlib/CMSIS/Device/ARM/ARMCM4/Include/ARMCM4.h

@@ -0,0 +1,265 @@
+/**************************************************************************//**
+ * @file     ARMCM4.h
+ * @brief    CMSIS Core Peripheral Access Layer Header File for
+ *           ARMCM4 Device Series
+ * @version  V1.07
+ * @date     30. January 2012
+ *
+ * @note
+ * Copyright (C) 2012 ARM Limited. All rights reserved.
+ *
+ * @par
+ * ARM Limited (ARM) is supplying this software for use with Cortex-M 
+ * processor based microcontrollers.  This file can be freely distributed 
+ * within development tools that are supporting such ARM based processors. 
+ *
+ * @par
+ * THIS SOFTWARE IS PROVIDED "AS IS".  NO WARRANTIES, WHETHER EXPRESS, IMPLIED
+ * OR STATUTORY, INCLUDING, BUT NOT LIMITED TO, IMPLIED WARRANTIES OF
+ * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE APPLY TO THIS SOFTWARE.
+ * ARM SHALL NOT, IN ANY CIRCUMSTANCES, BE LIABLE FOR SPECIAL, INCIDENTAL, OR
+ * CONSEQUENTIAL DAMAGES, FOR ANY REASON WHATSOEVER.
+ *
+ ******************************************************************************/
+
+#ifndef ARMCM4_H
+#define ARMCM4_H
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+
+/* -------------------------  Interrupt Number Definition  ------------------------ */
+
+typedef enum IRQn
+{
+/* -------------------  Cortex-M4 Processor Exceptions Numbers  ------------------- */
+  NonMaskableInt_IRQn           = -14,      /*!<  2 Non Maskable Interrupt          */
+  HardFault_IRQn                = -13,      /*!<  3 HardFault Interrupt             */
+  MemoryManagement_IRQn         = -12,      /*!<  4 Memory Management Interrupt     */
+  BusFault_IRQn                 = -11,      /*!<  5 Bus Fault Interrupt             */
+  UsageFault_IRQn               = -10,      /*!<  6 Usage Fault Interrupt           */
+  SVCall_IRQn                   =  -5,      /*!< 11 SV Call Interrupt               */
+  DebugMonitor_IRQn             =  -4,      /*!< 12 Debug Monitor Interrupt         */
+  PendSV_IRQn                   =  -2,      /*!< 14 Pend SV Interrupt               */
+  SysTick_IRQn                  =  -1,      /*!< 15 System Tick Interrupt           */
+
+/* ----------------------  ARMCM4 Specific Interrupt Numbers  --------------------- */
+  WDT_IRQn                      =   0,      /*!< Watchdog Timer Interrupt           */
+  RTC_IRQn                      =   1,      /*!< Real Time Clock Interrupt          */
+  TIM0_IRQn                     =   2,      /*!< Timer0 / Timer1 Interrupt          */
+  TIM2_IRQn                     =   3,      /*!< Timer2 / Timer3 Interrupt          */
+  MCIA_IRQn                     =   4,      /*!< MCIa Interrupt                     */
+  MCIB_IRQn                     =   5,      /*!< MCIb Interrupt                     */
+  UART0_IRQn                    =   6,      /*!< UART0 Interrupt                    */
+  UART1_IRQn                    =   7,      /*!< UART1 Interrupt                    */
+  UART2_IRQn                    =   8,      /*!< UART2 Interrupt                    */
+  UART4_IRQn                    =   9,      /*!< UART4 Interrupt                    */
+  AACI_IRQn                     =  10,      /*!< AACI / AC97 Interrupt              */
+  CLCD_IRQn                     =  11,      /*!< CLCD Combined Interrupt            */
+  ENET_IRQn                     =  12,      /*!< Ethernet Interrupt                 */
+  USBDC_IRQn                    =  13,      /*!< USB Device Interrupt               */
+  USBHC_IRQn                    =  14,      /*!< USB Host Controller Interrupt      */
+  CHLCD_IRQn                    =  15,      /*!< Character LCD Interrupt            */
+  FLEXRAY_IRQn                  =  16,      /*!< Flexray Interrupt                  */
+  CAN_IRQn                      =  17,      /*!< CAN Interrupt                      */
+  LIN_IRQn                      =  18,      /*!< LIN Interrupt                      */
+  I2C_IRQn                      =  19,      /*!< I2C ADC/DAC Interrupt              */
+  CPU_CLCD_IRQn                 =  28,      /*!< CPU CLCD Combined Interrupt        */
+  UART3_IRQn                    =  30,      /*!< UART3 Interrupt                    */
+  SPI_IRQn                      =  31,      /*!< SPI Touchscreen Interrupt          */
+} IRQn_Type;
+
+
+/* ================================================================================ */
+/* ================      Processor and Core Peripheral Section     ================ */
+/* ================================================================================ */
+
+/* --------  Configuration of the Cortex-M4 Processor and Core Peripherals  ------- */
+#define __CM4_REV                 0x0001    /*!< Core revision r0p1                              */
+#define __MPU_PRESENT             1         /*!< MPU present or not                              */
+#define __NVIC_PRIO_BITS          3         /*!< Number of Bits used for Priority Levels         */
+#define __Vendor_SysTickConfig    0         /*!< Set to 1 if different SysTick Config is used    */
+#define __FPU_PRESENT             1         /*!< FPU present or not                              */
+
+#include <core_cm4.h>                       /* Processor and core peripherals                    */
+/* NuttX */
+//#include "system_ARMCM4.h"                  /* System Header                                     */
+
+
+/* ================================================================================ */
+/* ================       Device Specific Peripheral Section       ================ */
+/* ================================================================================ */
+
+/* -------------------  Start of section using anonymous unions  ------------------ */
+#if defined(__CC_ARM)
+  #pragma push
+  #pragma anon_unions
+#elif defined(__ICCARM__)
+  #pragma language=extended
+#elif defined(__GNUC__)
+  /* anonymous unions are enabled by default */
+#elif defined(__TMS470__)
+/* anonymous unions are enabled by default */
+#elif defined(__TASKING__)
+  #pragma warning 586
+#else
+  #warning Not supported compiler type
+#endif
+
+
+
+/* ================================================================================ */
+/* ================            CPU FPGA System (CPU_SYS)           ================ */
+/* ================================================================================ */
+typedef struct
+{
+  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */
+  __IO uint32_t MEMCFG;           /* Offset: 0x004 (R/W)  Remap and Alias Memory Control */
+  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */
+  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */
+  __I  uint32_t TS;               /* Offset: 0x010 (R/ )  Touchscreen Register */
+  __IO uint32_t CTRL1;            /* Offset: 0x014 (R/W)  Misc Control Functions */
+       uint32_t RESERVED0[2];
+  __IO uint32_t CLKCFG;           /* Offset: 0x020 (R/W)  System Clock Configuration */
+  __IO uint32_t WSCFG;            /* Offset: 0x024 (R/W)  Flash Waitstate Configuration */
+  __IO uint32_t CPUCFG;           /* Offset: 0x028 (R/W)  Processor Configuration */
+       uint32_t RESERVED1[3];
+  __IO uint32_t BASE;             /* Offset: 0x038 (R/W)  ROM Table base Address */
+  __IO uint32_t ID2;              /* Offset: 0x03C (R/W)  Secondary Identification Register */
+} ARM_CPU_SYS_TypeDef;
+
+
+/* ================================================================================ */
+/* ================            DUT FPGA System (DUT_SYS)           ================ */
+/* ================================================================================ */
+typedef struct
+{
+  __I  uint32_t ID;               /* Offset: 0x000 (R/ )  Board and FPGA Identifier */
+  __IO uint32_t PERCFG;           /* Offset: 0x004 (R/W)  Peripheral Control Signals */
+  __I  uint32_t SW;               /* Offset: 0x008 (R/ )  Switch States */
+  __IO uint32_t LED;              /* Offset: 0x00C (R/W)  LED Output States */
+  __IO uint32_t SEG7;             /* Offset: 0x010 (R/W)  7-segment LED Output States */
+  __I  uint32_t CNT25MHz;         /* Offset: 0x014 (R/ )  Freerunning counter incrementing at 25MHz */
+  __I  uint32_t CNT100Hz;         /* Offset: 0x018 (R/ )  Freerunning counter incrementing at 100Hz */
+} ARM_DUT_SYS_TypeDef;
+
+
+/* ================================================================================ */
+/* ================                   Timer (TIM)                  ================ */
+/* ================================================================================ */
+typedef struct
+{
+  __IO uint32_t Timer1Load;       /* Offset: 0x000 (R/W)  Timer 1 Load */
+  __I  uint32_t Timer1Value;      /* Offset: 0x004 (R/ )  Timer 1 Counter Current Value */
+  __IO uint32_t Timer1Control;    /* Offset: 0x008 (R/W)  Timer 1 Control */
+  __O  uint32_t Timer1IntClr;     /* Offset: 0x00C ( /W)  Timer 1 Interrupt Clear */
+  __I  uint32_t Timer1RIS;        /* Offset: 0x010 (R/ )  Timer 1 Raw Interrupt Status */
+  __I  uint32_t Timer1MIS;        /* Offset: 0x014 (R/ )  Timer 1 Masked Interrupt Status */
+  __IO uint32_t Timer1BGLoad;     /* Offset: 0x018 (R/W)  Background Load Register */
+       uint32_t RESERVED0[1];
+  __IO uint32_t Timer2Load;       /* Offset: 0x020 (R/W)  Timer 2 Load */
+  __I  uint32_t Timer2Value;      /* Offset: 0x024 (R/ )  Timer 2 Counter Current Value */
+  __IO uint32_t Timer2Control;    /* Offset: 0x028 (R/W)  Timer 2 Control */
+  __O  uint32_t Timer2IntClr;     /* Offset: 0x02C ( /W)  Timer 2 Interrupt Clear */
+  __I  uint32_t Timer2RIS;        /* Offset: 0x030 (R/ )  Timer 2 Raw Interrupt Status */
+  __I  uint32_t Timer2MIS;        /* Offset: 0x034 (R/ )  Timer 2 Masked Interrupt Status */
+  __IO uint32_t Timer2BGLoad;     /* Offset: 0x038 (R/W)  Background Load Register */
+} ARM_TIM_TypeDef;
+
+
+/* ================================================================================ */
+/* ============== Universal Asyncronous Receiver / Transmitter (UART) ============= */
+/* ================================================================================ */
+typedef struct
+{
+  __IO uint32_t DR;               /* Offset: 0x000 (R/W)  Data */ 
+  union {
+  __I  uint32_t RSR;              /* Offset: 0x000 (R/ )  Receive Status */
+  __O  uint32_t ECR;              /* Offset: 0x000 ( /W)  Error Clear */
+  };
+       uint32_t RESERVED0[4];
+  __IO uint32_t FR;               /* Offset: 0x018 (R/W)  Flags */
+       uint32_t RESERVED1[1];
+  __IO uint32_t ILPR;             /* Offset: 0x020 (R/W)  IrDA Low-power Counter */
+  __IO uint32_t IBRD;             /* Offset: 0x024 (R/W)  Interger Baud Rate */
+  __IO uint32_t FBRD;             /* Offset: 0x028 (R/W)  Fractional Baud Rate */
+  __IO uint32_t LCR_H;            /* Offset: 0x02C (R/W)  Line Control */
+  __IO uint32_t CR;               /* Offset: 0x030 (R/W)  Control */
+  __IO uint32_t IFLS;             /* Offset: 0x034 (R/W)  Interrupt FIFO Level Select */
+  __IO uint32_t IMSC;             /* Offset: 0x038 (R/W)  Interrupt Mask Set / Clear */
+  __IO uint32_t RIS;              /* Offset: 0x03C (R/W)  Raw Interrupt Status */
+  __IO uint32_t MIS;              /* Offset: 0x040 (R/W)  Masked Interrupt Status */
+  __O  uint32_t ICR;              /* Offset: 0x044 ( /W)  Interrupt Clear */
+  __IO uint32_t DMACR;            /* Offset: 0x048 (R/W)  DMA Control */
+} ARM_UART_TypeDef;
+
+
+/* --------------------  End of section using anonymous unions  ------------------- */
+#if defined(__CC_ARM)
+  #pragma pop
+#elif defined(__ICCARM__)
+  /* leave anonymous unions enabled */
+#elif defined(__GNUC__)
+  /* anonymous unions are enabled by default */
+#elif defined(__TMS470__)
+  /* anonymous unions are enabled by default */
+#elif defined(__TASKING__)
+  #pragma warning restore
+#else
+  #warning Not supported compiler type
+#endif
+
+
+
+
+/* ================================================================================ */
+/* ================              Peripheral memory map             ================ */
+/* ================================================================================ */
+/* --------------------------  CPU FPGA memory map  ------------------------------- */
+#define ARM_FLASH_BASE            (0x00000000UL)
+#define ARM_RAM_BASE              (0x20000000UL)
+#define ARM_RAM_FPGA_BASE         (0x1EFF0000UL)
+#define ARM_CPU_CFG_BASE          (0xDFFF0000UL)
+
+#define ARM_CPU_SYS_BASE          (ARM_CPU_CFG_BASE  + 0x00000)
+#define ARM_UART3_BASE            (ARM_CPU_CFG_BASE  + 0x05000)
+
+/* --------------------------  DUT FPGA memory map  ------------------------------- */
+#define ARM_APB_BASE              (0x40000000UL)
+#define ARM_AHB_BASE              (0x4FF00000UL)
+#define ARM_DMC_BASE              (0x60000000UL)
+#define ARM_SMC_BASE              (0xA0000000UL)
+
+#define ARM_TIM0_BASE             (ARM_APB_BASE      + 0x02000)
+#define ARM_TIM2_BASE             (ARM_APB_BASE      + 0x03000)
+#define ARM_DUT_SYS_BASE          (ARM_APB_BASE      + 0x04000)
+#define ARM_UART0_BASE            (ARM_APB_BASE      + 0x06000)
+#define ARM_UART1_BASE            (ARM_APB_BASE      + 0x07000)
+#define ARM_UART2_BASE            (ARM_APB_BASE      + 0x08000)
+#define ARM_UART4_BASE            (ARM_APB_BASE      + 0x09000)
+
+
+/* ================================================================================ */
+/* ================             Peripheral declaration             ================ */
+/* ================================================================================ */
+/* --------------------------  CPU FPGA Peripherals  ------------------------------ */
+#define ARM_CPU_SYS               ((ARM_CPU_SYS_TypeDef *)  ARM_CPU_SYS_BASE)
+#define ARM_UART3                 ((   ARM_UART_TypeDef *)    ARM_UART3_BASE)
+
+/* --------------------------  DUT FPGA Peripherals  ------------------------------ */
+#define ARM_DUT_SYS               ((ARM_DUT_SYS_TypeDef *)  ARM_DUT_SYS_BASE)
+#define ARM_TIM0                  ((    ARM_TIM_TypeDef *)     ARM_TIM0_BASE)
+#define ARM_TIM2                  ((    ARM_TIM_TypeDef *)     ARM_TIM2_BASE)
+#define ARM_UART0                 ((   ARM_UART_TypeDef *)    ARM_UART0_BASE)
+#define ARM_UART1                 ((   ARM_UART_TypeDef *)    ARM_UART1_BASE)
+#define ARM_UART2                 ((   ARM_UART_TypeDef *)    ARM_UART2_BASE)
+#define ARM_UART4                 ((   ARM_UART_TypeDef *)    ARM_UART4_BASE)
+
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif  /* ARMCM4_H */

src/lib/mathlib/CMSIS/Include/arm_common_tables.h

@@ -0,0 +1,93 @@
+/* ----------------------------------------------------------------------
+* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
+*
+* $Date:        17. January 2013
+* $Revision:    V1.4.1
+*
+* Project:      CMSIS DSP Library
+* Title:        arm_common_tables.h
+*
+* Description:  This file has extern declaration for common tables like Bitreverse, reciprocal etc which are used across different functions
+*
+* Target Processor: Cortex-M4/Cortex-M3
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*   - Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   - 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.
+*   - Neither the name of ARM LIMITED 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.
+* -------------------------------------------------------------------- */
+
+#ifndef _ARM_COMMON_TABLES_H
+#define _ARM_COMMON_TABLES_H
+
+#include "arm_math.h"
+
+extern const uint16_t armBitRevTable[1024];
+extern const q15_t armRecipTableQ15[64];
+extern const q31_t armRecipTableQ31[64];
+extern const q31_t realCoefAQ31[1024];
+extern const q31_t realCoefBQ31[1024];
+extern const float32_t twiddleCoef_16[32];
+extern const float32_t twiddleCoef_32[64];
+extern const float32_t twiddleCoef_64[128];
+extern const float32_t twiddleCoef_128[256];
+extern const float32_t twiddleCoef_256[512];
+extern const float32_t twiddleCoef_512[1024];
+extern const float32_t twiddleCoef_1024[2048];
+extern const float32_t twiddleCoef_2048[4096];
+extern const float32_t twiddleCoef_4096[8192];
+#define twiddleCoef twiddleCoef_4096
+extern const q31_t twiddleCoefQ31[6144];
+extern const q15_t twiddleCoefQ15[6144];
+extern const float32_t twiddleCoef_rfft_32[32];
+extern const float32_t twiddleCoef_rfft_64[64];
+extern const float32_t twiddleCoef_rfft_128[128];
+extern const float32_t twiddleCoef_rfft_256[256];
+extern const float32_t twiddleCoef_rfft_512[512];
+extern const float32_t twiddleCoef_rfft_1024[1024];
+extern const float32_t twiddleCoef_rfft_2048[2048];
+extern const float32_t twiddleCoef_rfft_4096[4096];
+
+
+#define ARMBITREVINDEXTABLE__16_TABLE_LENGTH ((uint16_t)20  )
+#define ARMBITREVINDEXTABLE__32_TABLE_LENGTH ((uint16_t)48  )
+#define ARMBITREVINDEXTABLE__64_TABLE_LENGTH ((uint16_t)56  )
+#define ARMBITREVINDEXTABLE_128_TABLE_LENGTH ((uint16_t)208 )
+#define ARMBITREVINDEXTABLE_256_TABLE_LENGTH ((uint16_t)440 )
+#define ARMBITREVINDEXTABLE_512_TABLE_LENGTH ((uint16_t)448 )
+#define ARMBITREVINDEXTABLE1024_TABLE_LENGTH ((uint16_t)1800)
+#define ARMBITREVINDEXTABLE2048_TABLE_LENGTH ((uint16_t)3808)
+#define ARMBITREVINDEXTABLE4096_TABLE_LENGTH ((uint16_t)4032)
+
+extern const uint16_t armBitRevIndexTable16[ARMBITREVINDEXTABLE__16_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable32[ARMBITREVINDEXTABLE__32_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable64[ARMBITREVINDEXTABLE__64_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable128[ARMBITREVINDEXTABLE_128_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable256[ARMBITREVINDEXTABLE_256_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable512[ARMBITREVINDEXTABLE_512_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable1024[ARMBITREVINDEXTABLE1024_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable2048[ARMBITREVINDEXTABLE2048_TABLE_LENGTH];
+extern const uint16_t armBitRevIndexTable4096[ARMBITREVINDEXTABLE4096_TABLE_LENGTH];
+
+#endif /*  ARM_COMMON_TABLES_H */

src/lib/mathlib/CMSIS/Include/arm_const_structs.h

@@ -0,0 +1,85 @@
+/* ----------------------------------------------------------------------
+* Copyright (C) 2010-2013 ARM Limited. All rights reserved.
+*
+* $Date:        17. January 2013
+* $Revision:    V1.4.1
+*
+* Project:      CMSIS DSP Library
+* Title:        arm_const_structs.h
+*
+* Description:  This file has constant structs that are initialized for
+*               user convenience.  For example, some can be given as
+*               arguments to the arm_cfft_f32() function.
+*
+* Target Processor: Cortex-M4/Cortex-M3
+*
+* Redistribution and use in source and binary forms, with or without
+* modification, are permitted provided that the following conditions
+* are met:
+*   - Redistributions of source code must retain the above copyright
+*     notice, this list of conditions and the following disclaimer.
+*   - 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.
+*   - Neither the name of ARM LIMITED 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.
+* -------------------------------------------------------------------- */
+
+#ifndef _ARM_CONST_STRUCTS_H
+#define _ARM_CONST_STRUCTS_H
+
+#include "arm_math.h"
+#include "arm_common_tables.h"
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len16 = {
+      16, twiddleCoef_16, armBitRevIndexTable16, ARMBITREVINDEXTABLE__16_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len32 = {
+      32, twiddleCoef_32, armBitRevIndexTable32, ARMBITREVINDEXTABLE__32_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len64 = {
+      64, twiddleCoef_64, armBitRevIndexTable64, ARMBITREVINDEXTABLE__64_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len128 = {
+      128, twiddleCoef_128, armBitRevIndexTable128, ARMBITREVINDEXTABLE_128_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len256 = {
+      256, twiddleCoef_256, armBitRevIndexTable256, ARMBITREVINDEXTABLE_256_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len512 = {
+      512, twiddleCoef_512, armBitRevIndexTable512, ARMBITREVINDEXTABLE_512_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len1024 = {
+      1024, twiddleCoef_1024, armBitRevIndexTable1024, ARMBITREVINDEXTABLE1024_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len2048 = {
+      2048, twiddleCoef_2048, armBitRevIndexTable2048, ARMBITREVINDEXTABLE2048_TABLE_LENGTH
+   };
+
+   const arm_cfft_instance_f32 arm_cfft_sR_f32_len4096 = {
+      4096, twiddleCoef_4096, armBitRevIndexTable4096, ARMBITREVINDEXTABLE4096_TABLE_LENGTH
+   };
+
+#endif

src/lib/mathlib/CMSIS/Include/core_cm4_simd.h

@@ -0,0 +1,673 @@
+/**************************************************************************//**
+ * @file     core_cm4_simd.h
+ * @brief    CMSIS Cortex-M4 SIMD Header File
+ * @version  V3.20
+ * @date     25. February 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+   All rights reserved.
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+   - Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   - 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.
+   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
+   ---------------------------------------------------------------------------*/
+
+
+#ifdef __cplusplus
+ extern "C" {
+#endif
+
+#ifndef __CORE_CM4_SIMD_H
+#define __CORE_CM4_SIMD_H
+
+
+/*******************************************************************************
+ *                Hardware Abstraction Layer
+ ******************************************************************************/
+
+
+/* ###################  Compiler specific Intrinsics  ########################### */
+/** \defgroup CMSIS_SIMD_intrinsics CMSIS SIMD Intrinsics
+  Access to dedicated SIMD instructions
+  @{
+*/
+
+#if   defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
+/* ARM armcc specific functions */
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+#define __SADD8                           __sadd8
+#define __QADD8                           __qadd8
+#define __SHADD8                          __shadd8
+#define __UADD8                           __uadd8
+#define __UQADD8                          __uqadd8
+#define __UHADD8                          __uhadd8
+#define __SSUB8                           __ssub8
+#define __QSUB8                           __qsub8
+#define __SHSUB8                          __shsub8
+#define __USUB8                           __usub8
+#define __UQSUB8                          __uqsub8
+#define __UHSUB8                          __uhsub8
+#define __SADD16                          __sadd16
+#define __QADD16                          __qadd16
+#define __SHADD16                         __shadd16
+#define __UADD16                          __uadd16
+#define __UQADD16                         __uqadd16
+#define __UHADD16                         __uhadd16
+#define __SSUB16                          __ssub16
+#define __QSUB16                          __qsub16
+#define __SHSUB16                         __shsub16
+#define __USUB16                          __usub16
+#define __UQSUB16                         __uqsub16
+#define __UHSUB16                         __uhsub16
+#define __SASX                            __sasx
+#define __QASX                            __qasx
+#define __SHASX                           __shasx
+#define __UASX                            __uasx
+#define __UQASX                           __uqasx
+#define __UHASX                           __uhasx
+#define __SSAX                            __ssax
+#define __QSAX                            __qsax
+#define __SHSAX                           __shsax
+#define __USAX                            __usax
+#define __UQSAX                           __uqsax
+#define __UHSAX                           __uhsax
+#define __USAD8                           __usad8
+#define __USADA8                          __usada8
+#define __SSAT16                          __ssat16
+#define __USAT16                          __usat16
+#define __UXTB16                          __uxtb16
+#define __UXTAB16                         __uxtab16
+#define __SXTB16                          __sxtb16
+#define __SXTAB16                         __sxtab16
+#define __SMUAD                           __smuad
+#define __SMUADX                          __smuadx
+#define __SMLAD                           __smlad
+#define __SMLADX                          __smladx
+#define __SMLALD                          __smlald
+#define __SMLALDX                         __smlaldx
+#define __SMUSD                           __smusd
+#define __SMUSDX                          __smusdx
+#define __SMLSD                           __smlsd
+#define __SMLSDX                          __smlsdx
+#define __SMLSLD                          __smlsld
+#define __SMLSLDX                         __smlsldx
+#define __SEL                             __sel
+#define __QADD                            __qadd
+#define __QSUB                            __qsub
+
+#define __PKHBT(ARG1,ARG2,ARG3)          ( ((((uint32_t)(ARG1))          ) & 0x0000FFFFUL) |  \
+                                           ((((uint32_t)(ARG2)) << (ARG3)) & 0xFFFF0000UL)  )
+
+#define __PKHTB(ARG1,ARG2,ARG3)          ( ((((uint32_t)(ARG1))          ) & 0xFFFF0000UL) |  \
+                                           ((((uint32_t)(ARG2)) >> (ARG3)) & 0x0000FFFFUL)  )
+
+#define __SMMLA(ARG1,ARG2,ARG3)          ( (int32_t)((((int64_t)(ARG1) * (ARG2)) + \
+                                                      ((int64_t)(ARG3) << 32)      ) >> 32))
+
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+
+#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
+/* IAR iccarm specific functions */
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+#include <cmsis_iar.h>
+
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+
+#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
+/* TI CCS specific functions */
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+#include <cmsis_ccs.h>
+
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+
+#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
+/* GNU gcc specific functions */
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("sadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("shadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uqadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uhadd8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("ssub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("shsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("usub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uqsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uhsub8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SADD16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("sadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHADD16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("shadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UADD16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQADD16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uqadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHADD16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uhadd16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSUB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("ssub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSUB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("shsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USUB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("usub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSUB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uqsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSUB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uhsub16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SASX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("sasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QASX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHASX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("shasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UASX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQASX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uqasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHASX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uhasx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SSAX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("ssax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSAX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SHSAX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("shsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("usax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UQSAX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uqsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UHSAX(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uhsax %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USAD8(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("usad8 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __USADA8(uint32_t op1, uint32_t op2, uint32_t op3)
+{
+  uint32_t result;
+
+  __ASM volatile ("usada8 %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+  return(result);
+}
+
+#define __SSAT16(ARG1,ARG2) \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1); \
+  __ASM ("ssat16 %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
+  __RES; \
+ })
+
+#define __USAT16(ARG1,ARG2) \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1); \
+  __ASM ("usat16 %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
+  __RES; \
+ })
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTB16(uint32_t op1)
+{
+  uint32_t result;
+
+  __ASM volatile ("uxtb16 %0, %1" : "=r" (result) : "r" (op1));
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __UXTAB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("uxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTB16(uint32_t op1)
+{
+  uint32_t result;
+
+  __ASM volatile ("sxtb16 %0, %1" : "=r" (result) : "r" (op1));
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SXTAB16(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("sxtab16 %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUAD  (uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("smuad %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUADX (uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("smuadx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLAD (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+  uint32_t result;
+
+  __ASM volatile ("smlad %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLADX (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+  uint32_t result;
+
+  __ASM volatile ("smladx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+  return(result);
+}
+
+#define __SMLALD(ARG1,ARG2,ARG3) \
+({ \
+  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \
+  __ASM volatile ("smlald %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
+  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
+ })
+
+#define __SMLALDX(ARG1,ARG2,ARG3) \
+({ \
+  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((uint64_t)(ARG3) >> 32), __ARG3_L = (uint32_t)((uint64_t)(ARG3) & 0xFFFFFFFFUL); \
+  __ASM volatile ("smlaldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
+  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
+ })
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSD  (uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("smusd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMUSDX (uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("smusdx %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSD (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+  uint32_t result;
+
+  __ASM volatile ("smlsd %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMLSDX (uint32_t op1, uint32_t op2, uint32_t op3)
+{
+  uint32_t result;
+
+  __ASM volatile ("smlsdx %0, %1, %2, %3" : "=r" (result) : "r" (op1), "r" (op2), "r" (op3) );
+  return(result);
+}
+
+#define __SMLSLD(ARG1,ARG2,ARG3) \
+({ \
+  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \
+  __ASM volatile ("smlsld %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
+  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
+ })
+
+#define __SMLSLDX(ARG1,ARG2,ARG3) \
+({ \
+  uint32_t __ARG1 = (ARG1), __ARG2 = (ARG2), __ARG3_H = (uint32_t)((ARG3) >> 32), __ARG3_L = (uint32_t)((ARG3) & 0xFFFFFFFFUL); \
+  __ASM volatile ("smlsldx %0, %1, %2, %3" : "=r" (__ARG3_L), "=r" (__ARG3_H) : "r" (__ARG1), "r" (__ARG2), "0" (__ARG3_L), "1" (__ARG3_H) ); \
+  (uint64_t)(((uint64_t)__ARG3_H << 32) | __ARG3_L); \
+ })
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SEL  (uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("sel %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QADD(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qadd %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __QSUB(uint32_t op1, uint32_t op2)
+{
+  uint32_t result;
+
+  __ASM volatile ("qsub %0, %1, %2" : "=r" (result) : "r" (op1), "r" (op2) );
+  return(result);
+}
+
+#define __PKHBT(ARG1,ARG2,ARG3) \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \
+  __ASM ("pkhbt %0, %1, %2, lsl %3" : "=r" (__RES) :  "r" (__ARG1), "r" (__ARG2), "I" (ARG3)  ); \
+  __RES; \
+ })
+
+#define __PKHTB(ARG1,ARG2,ARG3) \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1), __ARG2 = (ARG2); \
+  if (ARG3 == 0) \
+    __ASM ("pkhtb %0, %1, %2" : "=r" (__RES) :  "r" (__ARG1), "r" (__ARG2)  ); \
+  else \
+    __ASM ("pkhtb %0, %1, %2, asr %3" : "=r" (__RES) :  "r" (__ARG1), "r" (__ARG2), "I" (ARG3)  ); \
+  __RES; \
+ })
+
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __SMMLA (int32_t op1, int32_t op2, int32_t op3)
+{
+ int32_t result;
+
+ __ASM volatile ("smmla %0, %1, %2, %3" : "=r" (result): "r"  (op1), "r" (op2), "r" (op3) );
+ return(result);
+}
+
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+
+#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
+/* TASKING carm specific functions */
+
+
+/*------ CM4 SIMD Intrinsics -----------------------------------------------------*/
+/* not yet supported */
+/*-- End CM4 SIMD Intrinsics -----------------------------------------------------*/
+
+
+#endif
+
+/*@} end of group CMSIS_SIMD_intrinsics */
+
+
+#endif /* __CORE_CM4_SIMD_H */
+
+#ifdef __cplusplus
+}
+#endif

src/lib/mathlib/CMSIS/Include/core_cmFunc.h

@@ -0,0 +1,636 @@
+/**************************************************************************//**
+ * @file     core_cmFunc.h
+ * @brief    CMSIS Cortex-M Core Function Access Header File
+ * @version  V3.20
+ * @date     25. February 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+   All rights reserved.
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+   - Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   - 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.
+   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
+   ---------------------------------------------------------------------------*/
+
+
+#ifndef __CORE_CMFUNC_H
+#define __CORE_CMFUNC_H
+
+
+/* ###########################  Core Function Access  ########################### */
+/** \ingroup  CMSIS_Core_FunctionInterface
+    \defgroup CMSIS_Core_RegAccFunctions CMSIS Core Register Access Functions
+  @{
+ */
+
+#if   defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
+/* ARM armcc specific functions */
+
+#if (__ARMCC_VERSION < 400677)
+  #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
+#endif
+
+/* intrinsic void __enable_irq();     */
+/* intrinsic void __disable_irq();    */
+
+/** \brief  Get Control Register
+
+    This function returns the content of the Control Register.
+
+    \return               Control Register value
+ */
+__STATIC_INLINE uint32_t __get_CONTROL(void)
+{
+  register uint32_t __regControl         __ASM("control");
+  return(__regControl);
+}
+
+
+/** \brief  Set Control Register
+
+    This function writes the given value to the Control Register.
+
+    \param [in]    control  Control Register value to set
+ */
+__STATIC_INLINE void __set_CONTROL(uint32_t control)
+{
+  register uint32_t __regControl         __ASM("control");
+  __regControl = control;
+}
+
+
+/** \brief  Get IPSR Register
+
+    This function returns the content of the IPSR Register.
+
+    \return               IPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_IPSR(void)
+{
+  register uint32_t __regIPSR          __ASM("ipsr");
+  return(__regIPSR);
+}
+
+
+/** \brief  Get APSR Register
+
+    This function returns the content of the APSR Register.
+
+    \return               APSR Register value
+ */
+__STATIC_INLINE uint32_t __get_APSR(void)
+{
+  register uint32_t __regAPSR          __ASM("apsr");
+  return(__regAPSR);
+}
+
+
+/** \brief  Get xPSR Register
+
+    This function returns the content of the xPSR Register.
+
+    \return               xPSR Register value
+ */
+__STATIC_INLINE uint32_t __get_xPSR(void)
+{
+  register uint32_t __regXPSR          __ASM("xpsr");
+  return(__regXPSR);
+}
+
+
+/** \brief  Get Process Stack Pointer
+
+    This function returns the current value of the Process Stack Pointer (PSP).
+
+    \return               PSP Register value
+ */
+__STATIC_INLINE uint32_t __get_PSP(void)
+{
+  register uint32_t __regProcessStackPointer  __ASM("psp");
+  return(__regProcessStackPointer);
+}
+
+
+/** \brief  Set Process Stack Pointer
+
+    This function assigns the given value to the Process Stack Pointer (PSP).
+
+    \param [in]    topOfProcStack  Process Stack Pointer value to set
+ */
+__STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
+{
+  register uint32_t __regProcessStackPointer  __ASM("psp");
+  __regProcessStackPointer = topOfProcStack;
+}
+
+
+/** \brief  Get Main Stack Pointer
+
+    This function returns the current value of the Main Stack Pointer (MSP).
+
+    \return               MSP Register value
+ */
+__STATIC_INLINE uint32_t __get_MSP(void)
+{
+  register uint32_t __regMainStackPointer     __ASM("msp");
+  return(__regMainStackPointer);
+}
+
+
+/** \brief  Set Main Stack Pointer
+
+    This function assigns the given value to the Main Stack Pointer (MSP).
+
+    \param [in]    topOfMainStack  Main Stack Pointer value to set
+ */
+__STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
+{
+  register uint32_t __regMainStackPointer     __ASM("msp");
+  __regMainStackPointer = topOfMainStack;
+}
+
+
+/** \brief  Get Priority Mask
+
+    This function returns the current state of the priority mask bit from the Priority Mask Register.
+
+    \return               Priority Mask value
+ */
+__STATIC_INLINE uint32_t __get_PRIMASK(void)
+{
+  register uint32_t __regPriMask         __ASM("primask");
+  return(__regPriMask);
+}
+
+
+/** \brief  Set Priority Mask
+
+    This function assigns the given value to the Priority Mask Register.
+
+    \param [in]    priMask  Priority Mask
+ */
+__STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
+{
+  register uint32_t __regPriMask         __ASM("primask");
+  __regPriMask = (priMask);
+}
+
+
+#if       (__CORTEX_M >= 0x03)
+
+/** \brief  Enable FIQ
+
+    This function enables FIQ interrupts by clearing the F-bit in the CPSR.
+    Can only be executed in Privileged modes.
+ */
+#define __enable_fault_irq                __enable_fiq
+
+
+/** \brief  Disable FIQ
+
+    This function disables FIQ interrupts by setting the F-bit in the CPSR.
+    Can only be executed in Privileged modes.
+ */
+#define __disable_fault_irq               __disable_fiq
+
+
+/** \brief  Get Base Priority
+
+    This function returns the current value of the Base Priority register.
+
+    \return               Base Priority register value
+ */
+__STATIC_INLINE uint32_t  __get_BASEPRI(void)
+{
+  register uint32_t __regBasePri         __ASM("basepri");
+  return(__regBasePri);
+}
+
+
+/** \brief  Set Base Priority
+
+    This function assigns the given value to the Base Priority register.
+
+    \param [in]    basePri  Base Priority value to set
+ */
+__STATIC_INLINE void __set_BASEPRI(uint32_t basePri)
+{
+  register uint32_t __regBasePri         __ASM("basepri");
+  __regBasePri = (basePri & 0xff);
+}
+
+
+/** \brief  Get Fault Mask
+
+    This function returns the current value of the Fault Mask register.
+
+    \return               Fault Mask register value
+ */
+__STATIC_INLINE uint32_t __get_FAULTMASK(void)
+{
+  register uint32_t __regFaultMask       __ASM("faultmask");
+  return(__regFaultMask);
+}
+
+
+/** \brief  Set Fault Mask
+
+    This function assigns the given value to the Fault Mask register.
+
+    \param [in]    faultMask  Fault Mask value to set
+ */
+__STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+  register uint32_t __regFaultMask       __ASM("faultmask");
+  __regFaultMask = (faultMask & (uint32_t)1);
+}
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+#if       (__CORTEX_M == 0x04)
+
+/** \brief  Get FPSCR
+
+    This function returns the current value of the Floating Point Status/Control register.
+
+    \return               Floating Point Status/Control register value
+ */
+__STATIC_INLINE uint32_t __get_FPSCR(void)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+  register uint32_t __regfpscr         __ASM("fpscr");
+  return(__regfpscr);
+#else
+   return(0);
+#endif
+}
+
+
+/** \brief  Set FPSCR
+
+    This function assigns the given value to the Floating Point Status/Control register.
+
+    \param [in]    fpscr  Floating Point Status/Control value to set
+ */
+__STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+  register uint32_t __regfpscr         __ASM("fpscr");
+  __regfpscr = (fpscr);
+#endif
+}
+
+#endif /* (__CORTEX_M == 0x04) */
+
+
+#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
+/* IAR iccarm specific functions */
+
+#include <cmsis_iar.h>
+
+
+#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
+/* TI CCS specific functions */
+
+#include <cmsis_ccs.h>
+
+
+#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
+/* GNU gcc specific functions */
+
+/** \brief  Enable IRQ Interrupts
+
+  This function enables IRQ interrupts by clearing the I-bit in the CPSR.
+  Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_irq(void)
+{
+  __ASM volatile ("cpsie i" : : : "memory");
+}
+
+
+/** \brief  Disable IRQ Interrupts
+
+  This function disables IRQ interrupts by setting the I-bit in the CPSR.
+  Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_irq(void)
+{
+  __ASM volatile ("cpsid i" : : : "memory");
+}
+
+
+/** \brief  Get Control Register
+
+    This function returns the content of the Control Register.
+
+    \return               Control Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_CONTROL(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, control" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Set Control Register
+
+    This function writes the given value to the Control Register.
+
+    \param [in]    control  Control Register value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_CONTROL(uint32_t control)
+{
+  __ASM volatile ("MSR control, %0" : : "r" (control) : "memory");
+}
+
+
+/** \brief  Get IPSR Register
+
+    This function returns the content of the IPSR Register.
+
+    \return               IPSR Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_IPSR(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, ipsr" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Get APSR Register
+
+    This function returns the content of the APSR Register.
+
+    \return               APSR Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_APSR(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, apsr" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Get xPSR Register
+
+    This function returns the content of the xPSR Register.
+
+    \return               xPSR Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_xPSR(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, xpsr" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Get Process Stack Pointer
+
+    This function returns the current value of the Process Stack Pointer (PSP).
+
+    \return               PSP Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PSP(void)
+{
+  register uint32_t result;
+
+  __ASM volatile ("MRS %0, psp\n"  : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Set Process Stack Pointer
+
+    This function assigns the given value to the Process Stack Pointer (PSP).
+
+    \param [in]    topOfProcStack  Process Stack Pointer value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PSP(uint32_t topOfProcStack)
+{
+  __ASM volatile ("MSR psp, %0\n" : : "r" (topOfProcStack) : "sp");
+}
+
+
+/** \brief  Get Main Stack Pointer
+
+    This function returns the current value of the Main Stack Pointer (MSP).
+
+    \return               MSP Register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_MSP(void)
+{
+  register uint32_t result;
+
+  __ASM volatile ("MRS %0, msp\n" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Set Main Stack Pointer
+
+    This function assigns the given value to the Main Stack Pointer (MSP).
+
+    \param [in]    topOfMainStack  Main Stack Pointer value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_MSP(uint32_t topOfMainStack)
+{
+  __ASM volatile ("MSR msp, %0\n" : : "r" (topOfMainStack) : "sp");
+}
+
+
+/** \brief  Get Priority Mask
+
+    This function returns the current state of the priority mask bit from the Priority Mask Register.
+
+    \return               Priority Mask value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_PRIMASK(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, primask" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Set Priority Mask
+
+    This function assigns the given value to the Priority Mask Register.
+
+    \param [in]    priMask  Priority Mask
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_PRIMASK(uint32_t priMask)
+{
+  __ASM volatile ("MSR primask, %0" : : "r" (priMask) : "memory");
+}
+
+
+#if       (__CORTEX_M >= 0x03)
+
+/** \brief  Enable FIQ
+
+    This function enables FIQ interrupts by clearing the F-bit in the CPSR.
+    Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __enable_fault_irq(void)
+{
+  __ASM volatile ("cpsie f" : : : "memory");
+}
+
+
+/** \brief  Disable FIQ
+
+    This function disables FIQ interrupts by setting the F-bit in the CPSR.
+    Can only be executed in Privileged modes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __disable_fault_irq(void)
+{
+  __ASM volatile ("cpsid f" : : : "memory");
+}
+
+
+/** \brief  Get Base Priority
+
+    This function returns the current value of the Base Priority register.
+
+    \return               Base Priority register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_BASEPRI(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, basepri_max" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Set Base Priority
+
+    This function assigns the given value to the Base Priority register.
+
+    \param [in]    basePri  Base Priority value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_BASEPRI(uint32_t value)
+{
+  __ASM volatile ("MSR basepri, %0" : : "r" (value) : "memory");
+}
+
+
+/** \brief  Get Fault Mask
+
+    This function returns the current value of the Fault Mask register.
+
+    \return               Fault Mask register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FAULTMASK(void)
+{
+  uint32_t result;
+
+  __ASM volatile ("MRS %0, faultmask" : "=r" (result) );
+  return(result);
+}
+
+
+/** \brief  Set Fault Mask
+
+    This function assigns the given value to the Fault Mask register.
+
+    \param [in]    faultMask  Fault Mask value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FAULTMASK(uint32_t faultMask)
+{
+  __ASM volatile ("MSR faultmask, %0" : : "r" (faultMask) : "memory");
+}
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+#if       (__CORTEX_M == 0x04)
+
+/** \brief  Get FPSCR
+
+    This function returns the current value of the Floating Point Status/Control register.
+
+    \return               Floating Point Status/Control register value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __get_FPSCR(void)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+  uint32_t result;
+
+  /* Empty asm statement works as a scheduling barrier */
+  __ASM volatile ("");
+  __ASM volatile ("VMRS %0, fpscr" : "=r" (result) );
+  __ASM volatile ("");
+  return(result);
+#else
+   return(0);
+#endif
+}
+
+
+/** \brief  Set FPSCR
+
+    This function assigns the given value to the Floating Point Status/Control register.
+
+    \param [in]    fpscr  Floating Point Status/Control value to set
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __set_FPSCR(uint32_t fpscr)
+{
+#if (__FPU_PRESENT == 1) && (__FPU_USED == 1)
+  /* Empty asm statement works as a scheduling barrier */
+  __ASM volatile ("");
+  __ASM volatile ("VMSR fpscr, %0" : : "r" (fpscr) : "vfpcc");
+  __ASM volatile ("");
+#endif
+}
+
+#endif /* (__CORTEX_M == 0x04) */
+
+
+#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
+/* TASKING carm specific functions */
+
+/*
+ * The CMSIS functions have been implemented as intrinsics in the compiler.
+ * Please use "carm -?i" to get an up to date list of all instrinsics,
+ * Including the CMSIS ones.
+ */
+
+#endif
+
+/*@} end of CMSIS_Core_RegAccFunctions */
+
+
+#endif /* __CORE_CMFUNC_H */

src/lib/mathlib/CMSIS/Include/core_cmInstr.h

@@ -0,0 +1,688 @@
+/**************************************************************************//**
+ * @file     core_cmInstr.h
+ * @brief    CMSIS Cortex-M Core Instruction Access Header File
+ * @version  V3.20
+ * @date     05. March 2013
+ *
+ * @note
+ *
+ ******************************************************************************/
+/* Copyright (c) 2009 - 2013 ARM LIMITED
+
+   All rights reserved.
+   Redistribution and use in source and binary forms, with or without
+   modification, are permitted provided that the following conditions are met:
+   - Redistributions of source code must retain the above copyright
+     notice, this list of conditions and the following disclaimer.
+   - 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.
+   - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.
+   ---------------------------------------------------------------------------*/
+
+
+#ifndef __CORE_CMINSTR_H
+#define __CORE_CMINSTR_H
+
+
+/* ##########################  Core Instruction Access  ######################### */
+/** \defgroup CMSIS_Core_InstructionInterface CMSIS Core Instruction Interface
+  Access to dedicated instructions
+  @{
+*/
+
+#if   defined ( __CC_ARM ) /*------------------RealView Compiler -----------------*/
+/* ARM armcc specific functions */
+
+#if (__ARMCC_VERSION < 400677)
+  #error "Please use ARM Compiler Toolchain V4.0.677 or later!"
+#endif
+
+
+/** \brief  No Operation
+
+    No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+#define __NOP                             __nop
+
+
+/** \brief  Wait For Interrupt
+
+    Wait For Interrupt is a hint instruction that suspends execution
+    until one of a number of events occurs.
+ */
+#define __WFI                             __wfi
+
+
+/** \brief  Wait For Event
+
+    Wait For Event is a hint instruction that permits the processor to enter
+    a low-power state until one of a number of events occurs.
+ */
+#define __WFE                             __wfe
+
+
+/** \brief  Send Event
+
+    Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+#define __SEV                             __sev
+
+
+/** \brief  Instruction Synchronization Barrier
+
+    Instruction Synchronization Barrier flushes the pipeline in the processor,
+    so that all instructions following the ISB are fetched from cache or
+    memory, after the instruction has been completed.
+ */
+#define __ISB()                           __isb(0xF)
+
+
+/** \brief  Data Synchronization Barrier
+
+    This function acts as a special kind of Data Memory Barrier.
+    It completes when all explicit memory accesses before this instruction complete.
+ */
+#define __DSB()                           __dsb(0xF)
+
+
+/** \brief  Data Memory Barrier
+
+    This function ensures the apparent order of the explicit memory operations before
+    and after the instruction, without ensuring their completion.
+ */
+#define __DMB()                           __dmb(0xF)
+
+
+/** \brief  Reverse byte order (32 bit)
+
+    This function reverses the byte order in integer value.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+#define __REV                             __rev
+
+
+/** \brief  Reverse byte order (16 bit)
+
+    This function reverses the byte order in two unsigned short values.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".rev16_text"))) __STATIC_INLINE __ASM uint32_t __REV16(uint32_t value)
+{
+  rev16 r0, r0
+  bx lr
+}
+#endif
+
+/** \brief  Reverse byte order in signed short value
+
+    This function reverses the byte order in a signed short value with sign extension to integer.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+#ifndef __NO_EMBEDDED_ASM
+__attribute__((section(".revsh_text"))) __STATIC_INLINE __ASM int32_t __REVSH(int32_t value)
+{
+  revsh r0, r0
+  bx lr
+}
+#endif
+
+
+/** \brief  Rotate Right in unsigned value (32 bit)
+
+    This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+
+    \param [in]    value  Value to rotate
+    \param [in]    value  Number of Bits to rotate
+    \return               Rotated value
+ */
+#define __ROR                             __ror
+
+
+/** \brief  Breakpoint
+
+    This function causes the processor to enter Debug state.
+    Debug tools can use this to investigate system state when the instruction at a particular address is reached.
+
+    \param [in]    value  is ignored by the processor.
+                   If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value)                       __breakpoint(value)
+
+
+#if       (__CORTEX_M >= 0x03)
+
+/** \brief  Reverse bit order of value
+
+    This function reverses the bit order of the given value.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+#define __RBIT                            __rbit
+
+
+/** \brief  LDR Exclusive (8 bit)
+
+    This function performs a exclusive LDR command for 8 bit value.
+
+    \param [in]    ptr  Pointer to data
+    \return             value of type uint8_t at (*ptr)
+ */
+#define __LDREXB(ptr)                     ((uint8_t ) __ldrex(ptr))
+
+
+/** \brief  LDR Exclusive (16 bit)
+
+    This function performs a exclusive LDR command for 16 bit values.
+
+    \param [in]    ptr  Pointer to data
+    \return        value of type uint16_t at (*ptr)
+ */
+#define __LDREXH(ptr)                     ((uint16_t) __ldrex(ptr))
+
+
+/** \brief  LDR Exclusive (32 bit)
+
+    This function performs a exclusive LDR command for 32 bit values.
+
+    \param [in]    ptr  Pointer to data
+    \return        value of type uint32_t at (*ptr)
+ */
+#define __LDREXW(ptr)                     ((uint32_t ) __ldrex(ptr))
+
+
+/** \brief  STR Exclusive (8 bit)
+
+    This function performs a exclusive STR command for 8 bit values.
+
+    \param [in]  value  Value to store
+    \param [in]    ptr  Pointer to location
+    \return          0  Function succeeded
+    \return          1  Function failed
+ */
+#define __STREXB(value, ptr)              __strex(value, ptr)
+
+
+/** \brief  STR Exclusive (16 bit)
+
+    This function performs a exclusive STR command for 16 bit values.
+
+    \param [in]  value  Value to store
+    \param [in]    ptr  Pointer to location
+    \return          0  Function succeeded
+    \return          1  Function failed
+ */
+#define __STREXH(value, ptr)              __strex(value, ptr)
+
+
+/** \brief  STR Exclusive (32 bit)
+
+    This function performs a exclusive STR command for 32 bit values.
+
+    \param [in]  value  Value to store
+    \param [in]    ptr  Pointer to location
+    \return          0  Function succeeded
+    \return          1  Function failed
+ */
+#define __STREXW(value, ptr)              __strex(value, ptr)
+
+
+/** \brief  Remove the exclusive lock
+
+    This function removes the exclusive lock which is created by LDREX.
+
+ */
+#define __CLREX                           __clrex
+
+
+/** \brief  Signed Saturate
+
+    This function saturates a signed value.
+
+    \param [in]  value  Value to be saturated
+    \param [in]    sat  Bit position to saturate to (1..32)
+    \return             Saturated value
+ */
+#define __SSAT                            __ssat
+
+
+/** \brief  Unsigned Saturate
+
+    This function saturates an unsigned value.
+
+    \param [in]  value  Value to be saturated
+    \param [in]    sat  Bit position to saturate to (0..31)
+    \return             Saturated value
+ */
+#define __USAT                            __usat
+
+
+/** \brief  Count leading zeros
+
+    This function counts the number of leading zeros of a data value.
+
+    \param [in]  value  Value to count the leading zeros
+    \return             number of leading zeros in value
+ */
+#define __CLZ                             __clz
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+
+#elif defined ( __ICCARM__ ) /*------------------ ICC Compiler -------------------*/
+/* IAR iccarm specific functions */
+
+#include <cmsis_iar.h>
+
+
+#elif defined ( __TMS470__ ) /*---------------- TI CCS Compiler ------------------*/
+/* TI CCS specific functions */
+
+#include <cmsis_ccs.h>
+
+
+#elif defined ( __GNUC__ ) /*------------------ GNU Compiler ---------------------*/
+/* GNU gcc specific functions */
+
+/* Define macros for porting to both thumb1 and thumb2.
+ * For thumb1, use low register (r0-r7), specified by constrant "l"
+ * Otherwise, use general registers, specified by constrant "r" */
+#if defined (__thumb__) && !defined (__thumb2__)
+#define __CMSIS_GCC_OUT_REG(r) "=l" (r)
+#define __CMSIS_GCC_USE_REG(r) "l" (r)
+#else
+#define __CMSIS_GCC_OUT_REG(r) "=r" (r)
+#define __CMSIS_GCC_USE_REG(r) "r" (r)
+#endif
+
+/** \brief  No Operation
+
+    No Operation does nothing. This instruction can be used for code alignment purposes.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __NOP(void)
+{
+  __ASM volatile ("nop");
+}
+
+
+/** \brief  Wait For Interrupt
+
+    Wait For Interrupt is a hint instruction that suspends execution
+    until one of a number of events occurs.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFI(void)
+{
+  __ASM volatile ("wfi");
+}
+
+
+/** \brief  Wait For Event
+
+    Wait For Event is a hint instruction that permits the processor to enter
+    a low-power state until one of a number of events occurs.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __WFE(void)
+{
+  __ASM volatile ("wfe");
+}
+
+
+/** \brief  Send Event
+
+    Send Event is a hint instruction. It causes an event to be signaled to the CPU.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __SEV(void)
+{
+  __ASM volatile ("sev");
+}
+
+
+/** \brief  Instruction Synchronization Barrier
+
+    Instruction Synchronization Barrier flushes the pipeline in the processor,
+    so that all instructions following the ISB are fetched from cache or
+    memory, after the instruction has been completed.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __ISB(void)
+{
+  __ASM volatile ("isb");
+}
+
+
+/** \brief  Data Synchronization Barrier
+
+    This function acts as a special kind of Data Memory Barrier.
+    It completes when all explicit memory accesses before this instruction complete.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __DSB(void)
+{
+  __ASM volatile ("dsb");
+}
+
+
+/** \brief  Data Memory Barrier
+
+    This function ensures the apparent order of the explicit memory operations before
+    and after the instruction, without ensuring their completion.
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __DMB(void)
+{
+  __ASM volatile ("dmb");
+}
+
+
+/** \brief  Reverse byte order (32 bit)
+
+    This function reverses the byte order in integer value.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV(uint32_t value)
+{
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 5)
+  return __builtin_bswap32(value);
+#else
+  uint32_t result;
+
+  __ASM volatile ("rev %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+  return(result);
+#endif
+}
+
+
+/** \brief  Reverse byte order (16 bit)
+
+    This function reverses the byte order in two unsigned short values.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __REV16(uint32_t value)
+{
+  uint32_t result;
+
+  __ASM volatile ("rev16 %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+  return(result);
+}
+
+
+/** \brief  Reverse byte order in signed short value
+
+    This function reverses the byte order in a signed short value with sign extension to integer.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE int32_t __REVSH(int32_t value)
+{
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+  return (short)__builtin_bswap16(value);
+#else
+  uint32_t result;
+
+  __ASM volatile ("revsh %0, %1" : __CMSIS_GCC_OUT_REG (result) : __CMSIS_GCC_USE_REG (value) );
+  return(result);
+#endif
+}
+
+
+/** \brief  Rotate Right in unsigned value (32 bit)
+
+    This function Rotate Right (immediate) provides the value of the contents of a register rotated by a variable number of bits.
+
+    \param [in]    value  Value to rotate
+    \param [in]    value  Number of Bits to rotate
+    \return               Rotated value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __ROR(uint32_t op1, uint32_t op2)
+{
+  return (op1 >> op2) | (op1 << (32 - op2)); 
+}
+
+
+/** \brief  Breakpoint
+
+    This function causes the processor to enter Debug state.
+    Debug tools can use this to investigate system state when the instruction at a particular address is reached.
+
+    \param [in]    value  is ignored by the processor.
+                   If required, a debugger can use it to store additional information about the breakpoint.
+ */
+#define __BKPT(value)                       __ASM volatile ("bkpt "#value)
+
+
+#if       (__CORTEX_M >= 0x03)
+
+/** \brief  Reverse bit order of value
+
+    This function reverses the bit order of the given value.
+
+    \param [in]    value  Value to reverse
+    \return               Reversed value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __RBIT(uint32_t value)
+{
+  uint32_t result;
+
+   __ASM volatile ("rbit %0, %1" : "=r" (result) : "r" (value) );
+   return(result);
+}
+
+
+/** \brief  LDR Exclusive (8 bit)
+
+    This function performs a exclusive LDR command for 8 bit value.
+
+    \param [in]    ptr  Pointer to data
+    \return             value of type uint8_t at (*ptr)
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __LDREXB(volatile uint8_t *addr)
+{
+    uint32_t result;
+
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+   __ASM volatile ("ldrexb %0, %1" : "=r" (result) : "Q" (*addr) );
+#else
+    /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
+       accepted by assembler. So has to use following less efficient pattern.
+    */
+   __ASM volatile ("ldrexb %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
+#endif
+   return(result);
+}
+
+
+/** \brief  LDR Exclusive (16 bit)
+
+    This function performs a exclusive LDR command for 16 bit values.
+
+    \param [in]    ptr  Pointer to data
+    \return        value of type uint16_t at (*ptr)
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint16_t __LDREXH(volatile uint16_t *addr)
+{
+    uint32_t result;
+
+#if (__GNUC__ > 4) || (__GNUC__ == 4 && __GNUC_MINOR__ >= 8)
+   __ASM volatile ("ldrexh %0, %1" : "=r" (result) : "Q" (*addr) );
+#else
+    /* Prior to GCC 4.8, "Q" will be expanded to [rx, #0] which is not
+       accepted by assembler. So has to use following less efficient pattern.
+    */
+   __ASM volatile ("ldrexh %0, [%1]" : "=r" (result) : "r" (addr) : "memory" );
+#endif
+   return(result);
+}
+
+
+/** \brief  LDR Exclusive (32 bit)
+
+    This function performs a exclusive LDR command for 32 bit values.
+
+    \param [in]    ptr  Pointer to data
+    \return        value of type uint32_t at (*ptr)
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __LDREXW(volatile uint32_t *addr)
+{
+    uint32_t result;
+
+   __ASM volatile ("ldrex %0, %1" : "=r" (result) : "Q" (*addr) );
+   return(result);
+}
+
+
+/** \brief  STR Exclusive (8 bit)
+
+    This function performs a exclusive STR command for 8 bit values.
+
+    \param [in]  value  Value to store
+    \param [in]    ptr  Pointer to location
+    \return          0  Function succeeded
+    \return          1  Function failed
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXB(uint8_t value, volatile uint8_t *addr)
+{
+   uint32_t result;
+
+   __ASM volatile ("strexb %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
+   return(result);
+}
+
+
+/** \brief  STR Exclusive (16 bit)
+
+    This function performs a exclusive STR command for 16 bit values.
+
+    \param [in]  value  Value to store
+    \param [in]    ptr  Pointer to location
+    \return          0  Function succeeded
+    \return          1  Function failed
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXH(uint16_t value, volatile uint16_t *addr)
+{
+   uint32_t result;
+
+   __ASM volatile ("strexh %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
+   return(result);
+}
+
+
+/** \brief  STR Exclusive (32 bit)
+
+    This function performs a exclusive STR command for 32 bit values.
+
+    \param [in]  value  Value to store
+    \param [in]    ptr  Pointer to location
+    \return          0  Function succeeded
+    \return          1  Function failed
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint32_t __STREXW(uint32_t value, volatile uint32_t *addr)
+{
+   uint32_t result;
+
+   __ASM volatile ("strex %0, %2, %1" : "=&r" (result), "=Q" (*addr) : "r" (value) );
+   return(result);
+}
+
+
+/** \brief  Remove the exclusive lock
+
+    This function removes the exclusive lock which is created by LDREX.
+
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE void __CLREX(void)
+{
+  __ASM volatile ("clrex" ::: "memory");
+}
+
+
+/** \brief  Signed Saturate
+
+    This function saturates a signed value.
+
+    \param [in]  value  Value to be saturated
+    \param [in]    sat  Bit position to saturate to (1..32)
+    \return             Saturated value
+ */
+#define __SSAT(ARG1,ARG2) \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1); \
+  __ASM ("ssat %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
+  __RES; \
+ })
+
+
+/** \brief  Unsigned Saturate
+
+    This function saturates an unsigned value.
+
+    \param [in]  value  Value to be saturated
+    \param [in]    sat  Bit position to saturate to (0..31)
+    \return             Saturated value
+ */
+#define __USAT(ARG1,ARG2) \
+({                          \
+  uint32_t __RES, __ARG1 = (ARG1); \
+  __ASM ("usat %0, %1, %2" : "=r" (__RES) :  "I" (ARG2), "r" (__ARG1) ); \
+  __RES; \
+ })
+
+
+/** \brief  Count leading zeros
+
+    This function counts the number of leading zeros of a data value.
+
+    \param [in]  value  Value to count the leading zeros
+    \return             number of leading zeros in value
+ */
+__attribute__( ( always_inline ) ) __STATIC_INLINE uint8_t __CLZ(uint32_t value)
+{
+   uint32_t result;
+
+  __ASM volatile ("clz %0, %1" : "=r" (result) : "r" (value) );
+  return(result);
+}
+
+#endif /* (__CORTEX_M >= 0x03) */
+
+
+
+
+#elif defined ( __TASKING__ ) /*------------------ TASKING Compiler --------------*/
+/* TASKING carm specific functions */
+
+/*
+ * The CMSIS functions have been implemented as intrinsics in the compiler.
+ * Please use "carm -?i" to get an up to date list of all intrinsics,
+ * Including the CMSIS ones.
+ */
+
+#endif
+
+/*@}*/ /* end of group CMSIS_Core_InstructionInterface */
+
+#endif /* __CORE_CMINSTR_H */

src/lib/mathlib/CMSIS/library.mk

@@ -0,0 +1,46 @@
+############################################################################
+#
+#   Copyright (c) 2013 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.
+#
+############################################################################
+
+#
+# ARM CMSIS DSP library
+#
+
+ifeq ($(CONFIG_ARCH),CORTEXM4F)
+PREBUILT_LIB		:= libarm_cortexM4lf_math.a
+else ifeq ($(CONFIG_ARCH),CORTEXM4)
+PREBUILT_LIB		:= libarm_cortexM4l_math.a
+else ifeq ($(CONFIG_ARCH),CORTEXM3)
+PREBUILT_LIB		:= libarm_cortexM3l_math.a
+else
+$(error CONFIG_ARCH value '$(CONFIG_ARCH)' not supported by the DSP library)
+endif

src/lib/mathlib/CMSIS/license.txt

@@ -0,0 +1,27 @@
+All pre-built libraries are guided by the following license:
+
+Copyright (C) 2009-2012 ARM Limited. 
+All rights reserved.
+
+Redistribution and use in source and binary forms, with or without
+modification, are permitted provided that the following conditions are met:
+ - Redistributions of source code must retain the above copyright
+   notice, this list of conditions and the following disclaimer.
+ - 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.
+ - Neither the name of ARM 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 COPYRIGHT HOLDERS AND 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.

src/lib/mathlib/math/Dcm.cpp

@@ -0,0 +1,179 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Dcm.cpp
+ *
+ * math direction cosine matrix
+ */
+
+#include <mathlib/math/test/test.hpp>
+
+#include "Dcm.hpp"
+#include "Quaternion.hpp"
+#include "EulerAngles.hpp"
+#include "Vector3.hpp"
+
+namespace math
+{
+
+Dcm::Dcm() :
+	Matrix(Matrix::identity(3))
+{
+}
+
+Dcm::Dcm(float c00, float c01, float c02,
+	 float c10, float c11, float c12,
+	 float c20, float c21, float c22) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	dcm(0, 0) = c00;
+	dcm(0, 1) = c01;
+	dcm(0, 2) = c02;
+	dcm(1, 0) = c10;
+	dcm(1, 1) = c11;
+	dcm(1, 2) = c12;
+	dcm(2, 0) = c20;
+	dcm(2, 1) = c21;
+	dcm(2, 2) = c22;
+}
+
+Dcm::Dcm(const float data[3][3]) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	/* set rotation matrix */
+	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+		dcm(i, j) = data[i][j];
+}
+
+Dcm::Dcm(const float *data) :
+	Matrix(3, 3, data)
+{
+}
+
+Dcm::Dcm(const Quaternion &q) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	double a = q.getA();
+	double b = q.getB();
+	double c = q.getC();
+	double d = q.getD();
+	double aSq = a * a;
+	double bSq = b * b;
+	double cSq = c * c;
+	double dSq = d * d;
+	dcm(0, 0) = aSq + bSq - cSq - dSq;
+	dcm(0, 1) = 2.0 * (b * c - a * d);
+	dcm(0, 2) = 2.0 * (a * c + b * d);
+	dcm(1, 0) = 2.0 * (b * c + a * d);
+	dcm(1, 1) = aSq - bSq + cSq - dSq;
+	dcm(1, 2) = 2.0 * (c * d - a * b);
+	dcm(2, 0) = 2.0 * (b * d - a * c);
+	dcm(2, 1) = 2.0 * (a * b + c * d);
+	dcm(2, 2) = aSq - bSq - cSq + dSq;
+}
+
+Dcm::Dcm(const EulerAngles &euler) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	double cosPhi = cos(euler.getPhi());
+	double sinPhi = sin(euler.getPhi());
+	double cosThe = cos(euler.getTheta());
+	double sinThe = sin(euler.getTheta());
+	double cosPsi = cos(euler.getPsi());
+	double sinPsi = sin(euler.getPsi());
+
+	dcm(0, 0) = cosThe * cosPsi;
+	dcm(0, 1) = -cosPhi * sinPsi + sinPhi * sinThe * cosPsi;
+	dcm(0, 2) = sinPhi * sinPsi + cosPhi * sinThe * cosPsi;
+
+	dcm(1, 0) = cosThe * sinPsi;
+	dcm(1, 1) = cosPhi * cosPsi + sinPhi * sinThe * sinPsi;
+	dcm(1, 2) = -sinPhi * cosPsi + cosPhi * sinThe * sinPsi;
+
+	dcm(2, 0) = -sinThe;
+	dcm(2, 1) = sinPhi * cosThe;
+	dcm(2, 2) = cosPhi * cosThe;
+}
+
+Dcm::Dcm(const Dcm &right) :
+	Matrix(right)
+{
+}
+
+Dcm::Dcm(const Matrix &right) :
+	Matrix(right)
+{
+}
+
+Dcm::~Dcm()
+{
+}
+
+int __EXPORT dcmTest()
+{
+	printf("Test DCM\t\t: ");
+	// default ctor
+	ASSERT(matrixEqual(Dcm(),
+			   Matrix::identity(3)));
+	// quaternion ctor
+	ASSERT(matrixEqual(
+		       Dcm(Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)),
+		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
+			   0.2896295f,  0.9564251f, -0.0369570f,
+			   -0.1986693f,  0.0978434f,  0.9751703f)));
+	// euler angle ctor
+	ASSERT(matrixEqual(
+		       Dcm(EulerAngles(0.1f, 0.2f, 0.3f)),
+		       Dcm(0.9362934f, -0.2750958f,  0.2183507f,
+			   0.2896295f,  0.9564251f, -0.0369570f,
+			   -0.1986693f,  0.0978434f,  0.9751703f)));
+	// rotations
+	Vector3 vB(1, 2, 3);
+	ASSERT(vectorEqual(Vector3(-2.0f, 1.0f, 3.0f),
+			   Dcm(EulerAngles(0.0f, 0.0f, M_PI_2_F))*vB));
+	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
+			   Dcm(EulerAngles(0.0f, M_PI_2_F, 0.0f))*vB));
+	ASSERT(vectorEqual(Vector3(1.0f, -3.0f, 2.0f),
+			   Dcm(EulerAngles(M_PI_2_F, 0.0f, 0.0f))*vB));
+	ASSERT(vectorEqual(Vector3(3.0f, 2.0f, -1.0f),
+			   Dcm(EulerAngles(
+				       M_PI_2_F, M_PI_2_F, M_PI_2_F))*vB));
+	printf("PASS\n");
+	return 0;
+}
+} // namespace math

src/lib/mathlib/math/Dcm.hpp

@@ -0,0 +1,113 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Dcm.hpp
+ *
+ * math direction cosine matrix
+ */
+
+//#pragma once
+
+#include "Vector.hpp"
+#include "Matrix.hpp"
+
+namespace math
+{
+
+class Quaternion;
+class EulerAngles;
+
+/**
+ * This is a Tait Bryan, Body 3-2-1 sequence.
+ * (yaw)-(pitch)-(roll)
+ * The Dcm transforms a vector in the body frame
+ * to the navigation frame, typically represented
+ * as C_nb. C_bn can be obtained through use
+ * of the transpose() method.
+ */
+class __EXPORT Dcm : public Matrix
+{
+public:
+	/**
+	 * default ctor
+	 */
+	Dcm();
+
+	/**
+	 * scalar ctor
+	 */
+	Dcm(float c00, float c01, float c02,
+	    float c10, float c11, float c12,
+	    float c20, float c21, float c22);
+
+	/**
+	 * data ctor
+	 */
+	Dcm(const float *data);
+
+	/**
+	 * array ctor
+	 */
+	Dcm(const float data[3][3]);
+
+	/**
+	 * quaternion ctor
+	 */
+	Dcm(const Quaternion &q);
+
+	/**
+	 * euler angles ctor
+	 */
+	Dcm(const EulerAngles &euler);
+
+	/**
+	 * copy ctor (deep)
+	 */
+	Dcm(const Dcm &right);
+
+	/**
+	 * copy ctor (deep)
+	 */
+	Dcm(const Matrix &right);
+
+	/**
+	 * dtor
+	 */
+	virtual ~Dcm();
+};
+
+int __EXPORT dcmTest();
+
+} // math
+

src/lib/mathlib/math/EulerAngles.cpp

@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+#include "EulerAngles.hpp"
+#include "Quaternion.hpp"
+#include "Dcm.hpp"
+#include "Vector3.hpp"
+
+namespace math
+{
+
+EulerAngles::EulerAngles() :
+	Vector(3)
+{
+	setPhi(0.0f);
+	setTheta(0.0f);
+	setPsi(0.0f);
+}
+
+EulerAngles::EulerAngles(float phi, float theta, float psi) :
+	Vector(3)
+{
+	setPhi(phi);
+	setTheta(theta);
+	setPsi(psi);
+}
+
+EulerAngles::EulerAngles(const Quaternion &q) :
+	Vector(3)
+{
+	(*this) = EulerAngles(Dcm(q));
+}
+
+EulerAngles::EulerAngles(const Dcm &dcm) :
+	Vector(3)
+{
+	setTheta(asinf(-dcm(2, 0)));
+
+	if (fabsf(getTheta() - M_PI_2_F) < 1.0e-3f) {
+		setPhi(0.0f);
+		setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
+			      dcm(0, 2) + dcm(1, 1)) + getPhi());
+
+	} else if (fabsf(getTheta() + M_PI_2_F) < 1.0e-3f) {
+		setPhi(0.0f);
+		setPsi(atan2f(dcm(1, 2) - dcm(0, 1),
+			      dcm(0, 2) + dcm(1, 1)) - getPhi());
+
+	} else {
+		setPhi(atan2f(dcm(2, 1), dcm(2, 2)));
+		setPsi(atan2f(dcm(1, 0), dcm(0, 0)));
+	}
+}
+
+EulerAngles::~EulerAngles()
+{
+}
+
+int __EXPORT eulerAnglesTest()
+{
+	printf("Test EulerAngles\t: ");
+	EulerAngles euler(0.1f, 0.2f, 0.3f);
+
+	// test ctor
+	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
+	ASSERT(equal(euler.getPhi(), 0.1f));
+	ASSERT(equal(euler.getTheta(), 0.2f));
+	ASSERT(equal(euler.getPsi(), 0.3f));
+
+	// test dcm ctor
+	euler = Dcm(EulerAngles(0.1f, 0.2f, 0.3f));
+	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
+
+	// test quat ctor
+	euler = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
+	ASSERT(vectorEqual(Vector3(0.1f, 0.2f, 0.3f), euler));
+
+	// test assignment
+	euler.setPhi(0.4f);
+	euler.setTheta(0.5f);
+	euler.setPsi(0.6f);
+	ASSERT(vectorEqual(Vector3(0.4f, 0.5f, 0.6f), euler));
+
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace math

src/lib/mathlib/math/EulerAngles.hpp

@@ -0,0 +1,74 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+class Quaternion;
+class Dcm;
+
+class __EXPORT EulerAngles : public Vector
+{
+public:
+	EulerAngles();
+	EulerAngles(float phi, float theta, float psi);
+	EulerAngles(const Quaternion &q);
+	EulerAngles(const Dcm &dcm);
+	virtual ~EulerAngles();
+
+	// alias
+	void setPhi(float phi) { (*this)(0) = phi; }
+	void setTheta(float theta) { (*this)(1) = theta; }
+	void setPsi(float psi) { (*this)(2) = psi; }
+
+	// const accessors
+	const float &getPhi() const { return (*this)(0); }
+	const float &getTheta() const { return (*this)(1); }
+	const float &getPsi() const { return (*this)(2); }
+
+};
+
+int __EXPORT eulerAnglesTest();
+
+} // math
+

src/lib/mathlib/math/Limits.cpp

@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Limits.cpp
+ *
+ * Limiting / constrain helper functions
+ */
+
+
+#include <math.h>
+#include <stdint.h>
+
+#include "Limits.hpp"
+
+
+namespace math {
+
+
+float __EXPORT min(float val1, float val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+int __EXPORT min(int val1, int val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+unsigned __EXPORT min(unsigned val1, unsigned val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+uint64_t __EXPORT min(uint64_t val1, uint64_t val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+double __EXPORT min(double val1, double val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+float __EXPORT max(float val1, float val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+int __EXPORT max(int val1, int val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+unsigned __EXPORT max(unsigned val1, unsigned val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+uint64_t __EXPORT max(uint64_t val1, uint64_t val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+double __EXPORT max(double val1, double val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+
+float __EXPORT constrain(float val, float min, float max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+int __EXPORT constrain(int val, int min, int max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+double __EXPORT constrain(double val, double min, double max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+float __EXPORT radians(float degrees)
+{
+	return (degrees / 180.0f) * M_PI_F;
+}
+
+double __EXPORT radians(double degrees)
+{
+	return (degrees / 180.0) * M_PI;
+}
+
+float __EXPORT degrees(float radians)
+{
+	return (radians / M_PI_F) * 180.0f;
+}
+
+double __EXPORT degrees(double radians)
+{
+	return (radians / M_PI) * 180.0;
+}
+
+}

src/lib/mathlib/math/Limits.hpp

@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Limits.hpp
+ *
+ * Limiting / constrain helper functions
+ */
+
+#pragma once
+
+#include <nuttx/config.h>
+#include <stdint.h>
+
+namespace math {
+
+
+float __EXPORT min(float val1, float val2);
+
+int __EXPORT min(int val1, int val2);
+
+unsigned __EXPORT min(unsigned val1, unsigned val2);
+
+uint64_t __EXPORT min(uint64_t val1, uint64_t val2);
+
+double __EXPORT min(double val1, double val2);
+
+float __EXPORT max(float val1, float val2);
+
+int __EXPORT max(int val1, int val2);
+
+unsigned __EXPORT max(unsigned val1, unsigned val2);
+
+uint64_t __EXPORT max(uint64_t val1, uint64_t val2);
+
+double __EXPORT max(double val1, double val2);
+
+
+float __EXPORT constrain(float val, float min, float max);
+
+int __EXPORT constrain(int val, int min, int max);
+
+unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max);
+
+uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max);
+
+double __EXPORT constrain(double val, double min, double max);
+
+float __EXPORT radians(float degrees);
+
+double __EXPORT radians(double degrees);
+
+float __EXPORT degrees(float radians);
+
+double __EXPORT degrees(double radians);
+
+}

src/lib/mathlib/math/Matrix.cpp

@@ -0,0 +1,193 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.cpp
+ *
+ * matrix code
+ */
+
+#include "test/test.hpp"
+#include <math.h>
+
+#include "Matrix.hpp"
+
+namespace math
+{
+
+static const float data_testA[] = {
+	1, 2, 3,
+	4, 5, 6
+};
+static Matrix testA(2, 3, data_testA);
+
+static const float data_testB[] = {
+	0, 1, 3,
+	7, -1, 2
+};
+static Matrix testB(2, 3, data_testB);
+
+static const float data_testC[] = {
+	0, 1,
+	2, 1,
+	3, 2
+};
+static Matrix testC(3, 2, data_testC);
+
+static const float data_testD[] = {
+	0, 1, 2,
+	2, 1, 4,
+	5, 2, 0
+};
+static Matrix testD(3, 3, data_testD);
+
+static const float data_testE[] = {
+	1, -1, 2,
+	0, 2, 3,
+	2, -1, 1
+};
+static Matrix testE(3, 3, data_testE);
+
+static const float data_testF[] = {
+	3.777e006f, 2.915e007f, 0.000e000f,
+	2.938e007f, 2.267e008f, 0.000e000f,
+	0.000e000f, 0.000e000f, 6.033e008f
+};
+static Matrix testF(3, 3, data_testF);
+
+int __EXPORT matrixTest()
+{
+	matrixAddTest();
+	matrixSubTest();
+	matrixMultTest();
+	matrixInvTest();
+	matrixDivTest();
+	return 0;
+}
+
+int matrixAddTest()
+{
+	printf("Test Matrix Add\t\t: ");
+	Matrix r = testA + testB;
+	float data_test[] = {
+		1.0f, 3.0f, 6.0f,
+		11.0f, 4.0f, 8.0f
+	};
+	ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
+	printf("PASS\n");
+	return 0;
+}
+
+int matrixSubTest()
+{
+	printf("Test Matrix Sub\t\t: ");
+	Matrix r = testA - testB;
+	float data_test[] = {
+		1.0f, 1.0f, 0.0f,
+		-3.0f, 6.0f, 4.0f
+	};
+	ASSERT(matrixEqual(Matrix(2, 3, data_test), r));
+	printf("PASS\n");
+	return 0;
+}
+
+int matrixMultTest()
+{
+	printf("Test Matrix Mult\t: ");
+	Matrix r = testC * testB;
+	float data_test[] = {
+		7.0f, -1.0f,  2.0f,
+		7.0f,  1.0f,  8.0f,
+		14.0f,  1.0f, 13.0f
+	};
+	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
+	printf("PASS\n");
+	return 0;
+}
+
+int matrixInvTest()
+{
+	printf("Test Matrix Inv\t\t: ");
+	Matrix origF = testF;
+	Matrix r = testF.inverse();
+	float data_test[] = {
+		-0.0012518f,  0.0001610f, 0.0000000f,
+		0.0001622f, -0.0000209f, 0.0000000f,
+		0.0000000f,  0.0000000f, 1.6580e-9f
+	};
+	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
+	// make sure F in unchanged
+	ASSERT(matrixEqual(origF, testF));
+	printf("PASS\n");
+	return 0;
+}
+
+int matrixDivTest()
+{
+	printf("Test Matrix Div\t\t: ");
+	Matrix r = testD / testE;
+	float data_test[] = {
+		0.2222222f, 0.5555556f, -0.1111111f,
+		0.0f,       1.0f,         1.0,
+		-4.1111111f, 1.2222222f,  4.5555556f
+	};
+	ASSERT(matrixEqual(Matrix(3, 3, data_test), r));
+	printf("PASS\n");
+	return 0;
+}
+
+bool matrixEqual(const Matrix &a, const Matrix &b, float eps)
+{
+	if (a.getRows() != b.getRows()) {
+		printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
+		return false;
+
+	} else if (a.getCols() != b.getCols()) {
+		printf("column number not equal a: %d, b:%d\n", a.getCols(), b.getCols());
+		return false;
+	}
+
+	bool ret = true;
+
+	for (size_t i = 0; i < a.getRows(); i++)
+		for (size_t j = 0; j < a.getCols(); j++) {
+			if (!equal(a(i, j), b(i, j), eps)) {
+				printf("element mismatch (%d, %d)\n", i, j);
+				ret = false;
+			}
+		}
+
+	return ret;
+}
+
+} // namespace math

src/lib/mathlib/math/Matrix.hpp

@@ -0,0 +1,61 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.h
+ *
+ * matrix code
+ */
+
+#pragma once
+
+#include <nuttx/config.h>
+
+#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
+#include "arm/Matrix.hpp"
+#else
+#include "generic/Matrix.hpp"
+#endif
+
+namespace math
+{
+class Matrix;
+int matrixTest();
+int matrixAddTest();
+int matrixSubTest();
+int matrixMultTest();
+int matrixInvTest();
+int matrixDivTest();
+int matrixArmTest();
+bool matrixEqual(const Matrix &a, const Matrix &b, float eps = 1.0e-5f);
+} // namespace math

src/lib/mathlib/math/Quaternion.cpp

@@ -0,0 +1,174 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Quaternion.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+
+#include "Quaternion.hpp"
+#include "Dcm.hpp"
+#include "EulerAngles.hpp"
+
+namespace math
+{
+
+Quaternion::Quaternion() :
+	Vector(4)
+{
+	setA(1.0f);
+	setB(0.0f);
+	setC(0.0f);
+	setD(0.0f);
+}
+
+Quaternion::Quaternion(float a, float b,
+		       float c, float d) :
+	Vector(4)
+{
+	setA(a);
+	setB(b);
+	setC(c);
+	setD(d);
+}
+
+Quaternion::Quaternion(const float *data) :
+	Vector(4, data)
+{
+}
+
+Quaternion::Quaternion(const Vector &v) :
+	Vector(v)
+{
+}
+
+Quaternion::Quaternion(const Dcm &dcm) :
+	Vector(4)
+{
+	// avoiding singularities by not using
+	// division equations
+	setA(0.5 * sqrt(1.0 +
+			double(dcm(0, 0) + dcm(1, 1) + dcm(2, 2))));
+	setB(0.5 * sqrt(1.0 +
+			double(dcm(0, 0) - dcm(1, 1) - dcm(2, 2))));
+	setC(0.5 * sqrt(1.0 +
+			double(-dcm(0, 0) + dcm(1, 1) - dcm(2, 2))));
+	setD(0.5 * sqrt(1.0 +
+			double(-dcm(0, 0) - dcm(1, 1) + dcm(2, 2))));
+}
+
+Quaternion::Quaternion(const EulerAngles &euler) :
+	Vector(4)
+{
+	double cosPhi_2 = cos(double(euler.getPhi()) / 2.0);
+	double sinPhi_2 = sin(double(euler.getPhi()) / 2.0);
+	double cosTheta_2 = cos(double(euler.getTheta()) / 2.0);
+	double sinTheta_2 = sin(double(euler.getTheta()) / 2.0);
+	double cosPsi_2 = cos(double(euler.getPsi()) / 2.0);
+	double sinPsi_2 = sin(double(euler.getPsi()) / 2.0);
+	setA(cosPhi_2 * cosTheta_2 * cosPsi_2 +
+	     sinPhi_2 * sinTheta_2 * sinPsi_2);
+	setB(sinPhi_2 * cosTheta_2 * cosPsi_2 -
+	     cosPhi_2 * sinTheta_2 * sinPsi_2);
+	setC(cosPhi_2 * sinTheta_2 * cosPsi_2 +
+	     sinPhi_2 * cosTheta_2 * sinPsi_2);
+	setD(cosPhi_2 * cosTheta_2 * sinPsi_2 -
+	     sinPhi_2 * sinTheta_2 * cosPsi_2);
+}
+
+Quaternion::Quaternion(const Quaternion &right) :
+	Vector(right)
+{
+}
+
+Quaternion::~Quaternion()
+{
+}
+
+Vector Quaternion::derivative(const Vector &w)
+{
+#ifdef QUATERNION_ASSERT
+	ASSERT(w.getRows() == 3);
+#endif
+	float dataQ[] = {
+		getA(), -getB(), -getC(), -getD(),
+		getB(),  getA(), -getD(),  getC(),
+		getC(),  getD(),  getA(), -getB(),
+		getD(), -getC(),  getB(),  getA()
+	};
+	Vector v(4);
+	v(0) = 0.0f;
+	v(1) = w(0);
+	v(2) = w(1);
+	v(3) = w(2);
+	Matrix Q(4, 4, dataQ);
+	return Q * v * 0.5f;
+}
+
+int __EXPORT quaternionTest()
+{
+	printf("Test Quaternion\t\t: ");
+	// test default ctor
+	Quaternion q;
+	ASSERT(equal(q.getA(), 1.0f));
+	ASSERT(equal(q.getB(), 0.0f));
+	ASSERT(equal(q.getC(), 0.0f));
+	ASSERT(equal(q.getD(), 0.0f));
+	// test float ctor
+	q = Quaternion(0.1825742f, 0.3651484f, 0.5477226f, 0.7302967f);
+	ASSERT(equal(q.getA(), 0.1825742f));
+	ASSERT(equal(q.getB(), 0.3651484f));
+	ASSERT(equal(q.getC(), 0.5477226f));
+	ASSERT(equal(q.getD(), 0.7302967f));
+	// test euler ctor
+	q = Quaternion(EulerAngles(0.1f, 0.2f, 0.3f));
+	ASSERT(vectorEqual(q, Quaternion(0.983347f, 0.034271f, 0.106021f, 0.143572f)));
+	// test dcm ctor
+	q = Quaternion(Dcm());
+	ASSERT(vectorEqual(q, Quaternion(1.0f, 0.0f, 0.0f, 0.0f)));
+	// TODO test derivative
+	// test accessors
+	q.setA(0.1f);
+	q.setB(0.2f);
+	q.setC(0.3f);
+	q.setD(0.4f);
+	ASSERT(vectorEqual(q, Quaternion(0.1f, 0.2f, 0.3f, 0.4f)));
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace math

src/lib/mathlib/math/Quaternion.hpp

@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Quaternion.hpp
+ *
+ * math quaternion lib
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+#include "Matrix.hpp"
+
+namespace math
+{
+
+class Dcm;
+class EulerAngles;
+
+class __EXPORT Quaternion : public Vector
+{
+public:
+
+	/**
+	 * default ctor
+	 */
+	Quaternion();
+
+	/**
+	 * ctor from floats
+	 */
+	Quaternion(float a, float b, float c, float d);
+
+	/**
+	 * ctor from data
+	 */
+	Quaternion(const float *data);
+
+	/**
+	 * ctor from Vector
+	 */
+	Quaternion(const Vector &v);
+
+	/**
+	 * ctor from EulerAngles
+	 */
+	Quaternion(const EulerAngles &euler);
+
+	/**
+	 * ctor from Dcm
+	 */
+	Quaternion(const Dcm &dcm);
+
+	/**
+	 * deep copy ctor
+	 */
+	Quaternion(const Quaternion &right);
+
+	/**
+	 * dtor
+	 */
+	virtual ~Quaternion();
+
+	/**
+	 * derivative
+	 */
+	Vector derivative(const Vector &w);
+
+	/**
+	 * accessors
+	 */
+	void setA(float a) { (*this)(0) = a; }
+	void setB(float b) { (*this)(1) = b; }
+	void setC(float c) { (*this)(2) = c; }
+	void setD(float d) { (*this)(3) = d; }
+	const float &getA() const { return (*this)(0); }
+	const float &getB() const { return (*this)(1); }
+	const float &getC() const { return (*this)(2); }
+	const float &getD() const { return (*this)(3); }
+};
+
+int __EXPORT quaternionTest();
+} // math
+

src/lib/mathlib/math/Vector.cpp

@@ -0,0 +1,100 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+static const float data_testA[] = {1, 3};
+static const float data_testB[] = {4, 1};
+
+static Vector testA(2, data_testA);
+static Vector testB(2, data_testB);
+
+int __EXPORT vectorTest()
+{
+	vectorAddTest();
+	vectorSubTest();
+	return 0;
+}
+
+int vectorAddTest()
+{
+	printf("Test Vector Add\t\t: ");
+	Vector r = testA + testB;
+	float data_test[] = {5.0f, 4.0f};
+	ASSERT(vectorEqual(Vector(2, data_test), r));
+	printf("PASS\n");
+	return 0;
+}
+
+int vectorSubTest()
+{
+	printf("Test Vector Sub\t\t: ");
+	Vector r(2);
+	r = testA - testB;
+	float data_test[] = { -3.0f, 2.0f};
+	ASSERT(vectorEqual(Vector(2, data_test), r));
+	printf("PASS\n");
+	return 0;
+}
+
+bool vectorEqual(const Vector &a, const Vector &b, float eps)
+{
+	if (a.getRows() != b.getRows()) {
+		printf("row number not equal a: %d, b:%d\n", a.getRows(), b.getRows());
+		return false;
+	}
+
+	bool ret = true;
+
+	for (size_t i = 0; i < a.getRows(); i++) {
+		if (!equal(a(i), b(i), eps)) {
+			printf("element mismatch (%d)\n", i);
+			ret = false;
+		}
+	}
+
+	return ret;
+}
+
+} // namespace math

src/lib/mathlib/math/Vector.hpp

@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#include <nuttx/config.h>
+
+#if defined(CONFIG_ARCH_CORTEXM4) && defined(CONFIG_ARCH_FPU)
+#include "arm/Vector.hpp"
+#else
+#include "generic/Vector.hpp"
+#endif
+
+namespace math
+{
+class Vector;
+int __EXPORT vectorTest();
+int __EXPORT vectorAddTest();
+int __EXPORT vectorSubTest();
+bool vectorEqual(const Vector &a, const Vector &b, float eps = 1.0e-5f);
+} // math

src/lib/mathlib/math/Vector2f.cpp

@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector2f.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+#include "Vector2f.hpp"
+
+namespace math
+{
+
+Vector2f::Vector2f() :
+	Vector(2)
+{
+}
+
+Vector2f::Vector2f(const Vector &right) :
+	Vector(right)
+{
+#ifdef VECTOR_ASSERT
+	ASSERT(right.getRows() == 2);
+#endif
+}
+
+Vector2f::Vector2f(float x, float y) :
+	Vector(2)
+{
+	setX(x);
+	setY(y);
+}
+
+Vector2f::Vector2f(const float *data) :
+	Vector(2, data)
+{
+}
+
+Vector2f::~Vector2f()
+{
+}
+
+float Vector2f::cross(const Vector2f &b) const
+{
+	const Vector2f &a = *this;
+	return a(0)*b(1) - a(1)*b(0);
+}
+
+float Vector2f::operator %(const Vector2f &v) const
+{
+	return cross(v);
+}
+    
+float Vector2f::operator *(const Vector2f &v) const
+{
+    return dot(v);
+}
+
+int __EXPORT vector2fTest()
+{
+	printf("Test Vector2f\t\t: ");
+	// test float ctor
+	Vector2f v(1, 2);
+	ASSERT(equal(v(0), 1));
+	ASSERT(equal(v(1), 2));
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace math

src/lib/mathlib/math/Vector2f.hpp

@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector2f.hpp
+ *
+ * math 3 vector
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+class __EXPORT Vector2f :
+	public Vector
+{
+public:
+	Vector2f();
+	Vector2f(const Vector &right);
+	Vector2f(float x, float y);
+	Vector2f(const float *data);
+	virtual ~Vector2f();
+	float cross(const Vector2f &b) const;
+	float operator %(const Vector2f &v) const;
+    float operator *(const Vector2f &v) const;
+    inline Vector2f operator*(const float &right) const {
+		return Vector::operator*(right);
+	}
+
+	/**
+	 * accessors
+	 */
+	void setX(float x) { (*this)(0) = x; }
+	void setY(float y) { (*this)(1) = y; }
+	const float &getX() const { return (*this)(0); }
+	const float &getY() const { return (*this)(1); }
+};
+    
+class __EXPORT Vector2 :
+	public Vector2f
+{
+};
+
+int __EXPORT vector2fTest();
+} // math
+

src/lib/mathlib/math/Vector3.cpp

@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector3.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+#include "Vector3.hpp"
+
+namespace math
+{
+
+Vector3::Vector3() :
+	Vector(3)
+{
+}
+
+Vector3::Vector3(const Vector &right) :
+	Vector(right)
+{
+#ifdef VECTOR_ASSERT
+	ASSERT(right.getRows() == 3);
+#endif
+}
+
+Vector3::Vector3(float x, float y, float z) :
+	Vector(3)
+{
+	setX(x);
+	setY(y);
+	setZ(z);
+}
+
+Vector3::Vector3(const float *data) :
+	Vector(3, data)
+{
+}
+
+Vector3::~Vector3()
+{
+}
+
+Vector3 Vector3::cross(const Vector3 &b) const
+{
+	const Vector3 &a = *this;
+	Vector3 result;
+	result(0) = a(1) * b(2) - a(2) * b(1);
+	result(1) = a(2) * b(0) - a(0) * b(2);
+	result(2) = a(0) * b(1) - a(1) * b(0);
+	return result;
+}
+
+Vector3 Vector3::operator %(const Vector3 &v) const
+{
+	return cross(v);
+}
+    
+float Vector3::operator *(const Vector3 &v) const
+{
+    return dot(v);
+}
+
+int __EXPORT vector3Test()
+{
+	printf("Test Vector3\t\t: ");
+	// test float ctor
+	Vector3 v(1, 2, 3);
+	ASSERT(equal(v(0), 1));
+	ASSERT(equal(v(1), 2));
+	ASSERT(equal(v(2), 3));
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace math

src/lib/mathlib/math/Vector3.hpp

@@ -0,0 +1,78 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector3.hpp
+ *
+ * math 3 vector
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+class __EXPORT Vector3 :
+	public Vector
+{
+public:
+	Vector3();
+	Vector3(const Vector &right);
+	Vector3(float x, float y, float z);
+	Vector3(const float *data);
+	virtual ~Vector3();
+	Vector3 cross(const Vector3 &b) const;
+	Vector3 operator %(const Vector3 &v) const;
+	float operator *(const Vector3 &v) const;
+
+	/**
+	 * accessors
+	 */
+	void setX(float x) { (*this)(0) = x; }
+	void setY(float y) { (*this)(1) = y; }
+	void setZ(float z) { (*this)(2) = z; }
+	const float &getX() const { return (*this)(0); }
+	const float &getY() const { return (*this)(1); }
+	const float &getZ() const { return (*this)(2); }
+};
+    
+class __EXPORT Vector3f :
+	public Vector3
+{
+};
+
+int __EXPORT vector3Test();
+} // math
+

src/lib/mathlib/math/arm/Matrix.cpp

@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.cpp
+ *
+ * matrix code
+ */
+
+#include "Matrix.hpp"

src/lib/mathlib/math/arm/Matrix.hpp

@@ -0,0 +1,292 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.h
+ *
+ * matrix code
+ */
+
+#pragma once
+
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "../Vector.hpp"
+#include "../Matrix.hpp"
+
+// arm specific
+#include "../../CMSIS/Include/arm_math.h"
+
+namespace math
+{
+
+class __EXPORT Matrix
+{
+public:
+	// constructor
+	Matrix(size_t rows, size_t cols) :
+		_matrix() {
+		arm_mat_init_f32(&_matrix,
+				 rows, cols,
+				 (float *)calloc(rows * cols, sizeof(float)));
+	}
+	Matrix(size_t rows, size_t cols, const float *data) :
+		_matrix() {
+		arm_mat_init_f32(&_matrix,
+				 rows, cols,
+				 (float *)malloc(rows * cols * sizeof(float)));
+		memcpy(getData(), data, getSize());
+	}
+	// deconstructor
+	virtual ~Matrix() {
+		delete [] _matrix.pData;
+	}
+	// copy constructor (deep)
+	Matrix(const Matrix &right) :
+		_matrix() {
+		arm_mat_init_f32(&_matrix,
+				 right.getRows(), right.getCols(),
+				 (float *)malloc(right.getRows()*
+						 right.getCols()*sizeof(float)));
+		memcpy(getData(), right.getData(),
+		       getSize());
+	}
+	// assignment
+	inline Matrix &operator=(const Matrix &right) {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == right.getRows());
+		ASSERT(getCols() == right.getCols());
+#endif
+
+		if (this != &right) {
+			memcpy(getData(), right.getData(),
+			       right.getSize());
+		}
+
+		return *this;
+	}
+	// element accessors
+	inline float &operator()(size_t i, size_t j) {
+#ifdef MATRIX_ASSERT
+		ASSERT(i < getRows());
+		ASSERT(j < getCols());
+#endif
+		return getData()[i * getCols() + j];
+	}
+	inline const float &operator()(size_t i, size_t j) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(i < getRows());
+		ASSERT(j < getCols());
+#endif
+		return getData()[i * getCols() + j];
+	}
+	// output
+	inline void print() const {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				float sig;
+				int exponent;
+				float num = (*this)(i, j);
+				float2SigExp(num, sig, exponent);
+				printf("%6.3fe%03d ", (double)sig, exponent);
+			}
+
+			printf("\n");
+		}
+	}
+	// boolean ops
+	inline bool operator==(const Matrix &right) const {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
+					return false;
+			}
+		}
+
+		return true;
+	}
+	// scalar ops
+	inline Matrix operator+(float right) const {
+		Matrix result(getRows(), getCols());
+		arm_offset_f32((float *)getData(), right,
+			       (float *)result.getData(), getRows()*getCols());
+		return result;
+	}
+	inline Matrix operator-(float right) const {
+		Matrix result(getRows(), getCols());
+		arm_offset_f32((float *)getData(), -right,
+			       (float *)result.getData(), getRows()*getCols());
+		return result;
+	}
+	inline Matrix operator*(float right) const {
+		Matrix result(getRows(), getCols());
+		arm_mat_scale_f32(&_matrix, right,
+				  &(result._matrix));
+		return result;
+	}
+	inline Matrix operator/(float right) const {
+		Matrix result(getRows(), getCols());
+		arm_mat_scale_f32(&_matrix, 1.0f / right,
+				  &(result._matrix));
+		return result;
+	}
+	// vector ops
+	inline Vector operator*(const Vector &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getCols() == right.getRows());
+#endif
+		Matrix resultMat = (*this) *
+				   Matrix(right.getRows(), 1, right.getData());
+		return Vector(getRows(), resultMat.getData());
+	}
+	// matrix ops
+	inline Matrix operator+(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == right.getRows());
+		ASSERT(getCols() == right.getCols());
+#endif
+		Matrix result(getRows(), getCols());
+		arm_mat_add_f32(&_matrix, &(right._matrix),
+				&(result._matrix));
+		return result;
+	}
+	inline Matrix operator-(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == right.getRows());
+		ASSERT(getCols() == right.getCols());
+#endif
+		Matrix result(getRows(), getCols());
+		arm_mat_sub_f32(&_matrix, &(right._matrix),
+				&(result._matrix));
+		return result;
+	}
+	inline Matrix operator*(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getCols() == right.getRows());
+#endif
+		Matrix result(getRows(), right.getCols());
+		arm_mat_mult_f32(&_matrix, &(right._matrix),
+				 &(result._matrix));
+		return result;
+	}
+	inline Matrix operator/(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(right.getRows() == right.getCols());
+		ASSERT(getCols() == right.getCols());
+#endif
+		return (*this) * right.inverse();
+	}
+	// other functions
+	inline Matrix transpose() const {
+		Matrix result(getCols(), getRows());
+		arm_mat_trans_f32(&_matrix, &(result._matrix));
+		return result;
+	}
+	inline void swapRows(size_t a, size_t b) {
+		if (a == b) return;
+
+		for (size_t j = 0; j < getCols(); j++) {
+			float tmp = (*this)(a, j);
+			(*this)(a, j) = (*this)(b, j);
+			(*this)(b, j) = tmp;
+		}
+	}
+	inline void swapCols(size_t a, size_t b) {
+		if (a == b) return;
+
+		for (size_t i = 0; i < getRows(); i++) {
+			float tmp = (*this)(i, a);
+			(*this)(i, a) = (*this)(i, b);
+			(*this)(i, b) = tmp;
+		}
+	}
+	/**
+	 * inverse based on LU factorization with partial pivotting
+	 */
+	Matrix inverse() const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == getCols());
+#endif
+		Matrix result(getRows(), getCols());
+		Matrix work = (*this);
+		arm_mat_inverse_f32(&(work._matrix),
+				    &(result._matrix));
+		return result;
+	}
+	inline void setAll(const float &val) {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				(*this)(i, j) = val;
+			}
+		}
+	}
+	inline void set(const float *data) {
+		memcpy(getData(), data, getSize());
+	}
+	inline size_t getRows() const { return _matrix.numRows; }
+	inline size_t getCols() const { return _matrix.numCols; }
+	inline static Matrix identity(size_t size) {
+		Matrix result(size, size);
+
+		for (size_t i = 0; i < size; i++) {
+			result(i, i) = 1.0f;
+		}
+
+		return result;
+	}
+	inline static Matrix zero(size_t size) {
+		Matrix result(size, size);
+		result.setAll(0.0f);
+		return result;
+	}
+	inline static Matrix zero(size_t m, size_t n) {
+		Matrix result(m, n);
+		result.setAll(0.0f);
+		return result;
+	}
+protected:
+	inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
+	inline float *getData() { return _matrix.pData; }
+	inline const float *getData() const { return _matrix.pData; }
+	inline void setData(float *data) { _matrix.pData = data; }
+private:
+	arm_matrix_instance_f32 _matrix;
+};
+
+} // namespace math

src/lib/mathlib/math/arm/Vector.cpp

@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.cpp
+ *
+ * math vector
+ */
+
+#include "Vector.hpp"

src/lib/mathlib/math/arm/Vector.hpp

@@ -0,0 +1,236 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "../Vector.hpp"
+#include "../test/test.hpp"
+
+// arm specific
+#include "../../CMSIS/Include/arm_math.h"
+
+namespace math
+{
+
+class __EXPORT Vector
+{
+public:
+	// constructor
+	Vector(size_t rows) :
+		_rows(rows),
+		_data((float *)calloc(rows, sizeof(float))) {
+	}
+	Vector(size_t rows, const float *data) :
+		_rows(rows),
+		_data((float *)malloc(getSize())) {
+		memcpy(getData(), data, getSize());
+	}
+	// deconstructor
+	virtual ~Vector() {
+		delete [] getData();
+	}
+	// copy constructor (deep)
+	Vector(const Vector &right) :
+		_rows(right.getRows()),
+		_data((float *)malloc(getSize())) {
+		memcpy(getData(), right.getData(),
+		       right.getSize());
+	}
+	// assignment
+	inline Vector &operator=(const Vector &right) {
+#ifdef VECTOR_ASSERT
+		ASSERT(getRows() == right.getRows());
+#endif
+
+		if (this != &right) {
+			memcpy(getData(), right.getData(),
+			       right.getSize());
+		}
+
+		return *this;
+	}
+	// element accessors
+	inline float &operator()(size_t i) {
+#ifdef VECTOR_ASSERT
+		ASSERT(i < getRows());
+#endif
+		return getData()[i];
+	}
+	inline const float &operator()(size_t i) const {
+#ifdef VECTOR_ASSERT
+		ASSERT(i < getRows());
+#endif
+		return getData()[i];
+	}
+	// output
+	inline void print() const {
+		for (size_t i = 0; i < getRows(); i++) {
+			float sig;
+			int exponent;
+			float num = (*this)(i);
+			float2SigExp(num, sig, exponent);
+			printf("%6.3fe%03d ", (double)sig, exponent);
+		}
+
+		printf("\n");
+	}
+	// boolean ops
+	inline bool operator==(const Vector &right) const {
+		for (size_t i = 0; i < getRows(); i++) {
+			if (fabsf(((*this)(i) - right(i))) > 1e-30f)
+				return false;
+		}
+
+		return true;
+	}
+	// scalar ops
+	inline Vector operator+(float right) const {
+		Vector result(getRows());
+		arm_offset_f32((float *)getData(),
+			       right, result.getData(),
+			       getRows());
+		return result;
+	}
+	inline Vector operator-(float right) const {
+		Vector result(getRows());
+		arm_offset_f32((float *)getData(),
+			       -right, result.getData(),
+			       getRows());
+		return result;
+	}
+	inline Vector operator*(const float &right) const {
+		Vector result(getRows());
+		arm_scale_f32((float *)getData(),
+			      right, result.getData(),
+			      getRows());
+		return result;
+	}
+	inline Vector operator/(float right) const {
+		Vector result(getRows());
+		arm_scale_f32((float *)getData(),
+			      1.0f / right, result.getData(),
+			      getRows());
+		return result;
+	}
+	// vector ops
+	inline Vector operator+(const Vector &right) const {
+#ifdef VECTOR_ASSERT
+		ASSERT(getRows() == right.getRows());
+#endif
+		Vector result(getRows());
+		arm_add_f32((float *)getData(),
+			    (float *)right.getData(),
+			    result.getData(),
+			    getRows());
+		return result;
+	}
+	inline Vector operator-(const Vector &right) const {
+#ifdef VECTOR_ASSERT
+		ASSERT(getRows() == right.getRows());
+#endif
+		Vector result(getRows());
+		arm_sub_f32((float *)getData(),
+			    (float *)right.getData(),
+			    result.getData(),
+			    getRows());
+		return result;
+	}
+	inline Vector operator-(void) const {
+		Vector result(getRows());
+		arm_negate_f32((float *)getData(),
+			    result.getData(),
+			    getRows());
+		return result;
+	}
+	// other functions
+	inline float dot(const Vector &right) const {
+		float result = 0;
+		arm_dot_prod_f32((float *)getData(),
+				 (float *)right.getData(),
+				 getRows(),
+				 &result);
+		return result;
+	}
+	inline float norm() const {
+		return sqrtf(dot(*this));
+	}
+	inline float length() const {
+		return norm();
+	}
+	inline Vector unit() const {
+		return (*this) / norm();
+	}
+	inline Vector normalized() const {
+		return unit();
+	}
+	inline void normalize() {
+		(*this) = (*this) / norm();
+	}
+	inline static Vector zero(size_t rows) {
+		Vector result(rows);
+		// calloc returns zeroed memory
+		return result;
+	}
+	inline void setAll(const float &val) {
+		for (size_t i = 0; i < getRows(); i++) {
+			(*this)(i) = val;
+		}
+	}
+	inline void set(const float *data) {
+		memcpy(getData(), data, getSize());
+	}
+	inline size_t getRows() const { return _rows; }
+	inline const float *getData() const { return _data; }
+protected:
+	inline size_t getSize() const { return sizeof(float) * getRows(); }
+	inline float *getData() { return _data; }
+	inline void setData(float *data) { _data = data; }
+private:
+	size_t _rows;
+	float *_data;
+};
+
+} // math

src/lib/mathlib/math/filter/LowPassFilter2p.cpp

@@ -0,0 +1,77 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/// @file	LowPassFilter.cpp
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+
+#include "LowPassFilter2p.hpp"
+#include "math.h"
+
+namespace math
+{
+
+void LowPassFilter2p::set_cutoff_frequency(float sample_freq, float cutoff_freq)
+{
+    _cutoff_freq = cutoff_freq;
+    float fr = sample_freq/_cutoff_freq;
+    float ohm = tanf(M_PI_F/fr);
+    float c = 1.0f+2.0f*cosf(M_PI_F/4.0f)*ohm + ohm*ohm;
+    _b0 = ohm*ohm/c;
+    _b1 = 2.0f*_b0;
+    _b2 = _b0;
+    _a1 = 2.0f*(ohm*ohm-1.0f)/c;
+    _a2 = (1.0f-2.0f*cosf(M_PI_F/4.0f)*ohm+ohm*ohm)/c;
+}
+
+float LowPassFilter2p::apply(float sample)
+{
+    // do the filtering
+    float delay_element_0 = sample - _delay_element_1 * _a1 - _delay_element_2 * _a2;
+    if (isnan(delay_element_0) || isinf(delay_element_0)) {
+        // don't allow bad values to propogate via the filter
+        delay_element_0 = sample;
+    }
+    float output = delay_element_0 * _b0 + _delay_element_1 * _b1 + _delay_element_2 * _b2;
+    
+    _delay_element_2 = _delay_element_1;
+    _delay_element_1 = delay_element_0;
+
+    // return the value.  Should be no need to check limits
+    return output;
+}
+
+} // namespace math
+

src/lib/mathlib/math/filter/LowPassFilter2p.hpp

@@ -0,0 +1,78 @@
+// -*- tab-width: 4; Mode: C++; c-basic-offset: 4; indent-tabs-mode: nil -*-
+
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/// @file	LowPassFilter.h
+/// @brief	A class to implement a second order low pass filter 
+/// Author: Leonard Hall <LeonardTHall@gmail.com>
+/// Adapted for PX4 by Andrew Tridgell
+
+#pragma once
+
+namespace math
+{
+class __EXPORT LowPassFilter2p
+{
+public:
+    // constructor
+    LowPassFilter2p(float sample_freq, float cutoff_freq) {
+        // set initial parameters
+        set_cutoff_frequency(sample_freq, cutoff_freq);
+        _delay_element_1 = _delay_element_2 = 0;
+    }
+
+    // change parameters
+    void set_cutoff_frequency(float sample_freq, float cutoff_freq);
+
+    // apply - Add a new raw value to the filter 
+    // and retrieve the filtered result
+    float apply(float sample);
+
+    // return the cutoff frequency
+    float get_cutoff_freq(void) const {
+        return _cutoff_freq;
+    }
+
+private:
+    float           _cutoff_freq; 
+    float           _a1;
+    float           _a2;
+    float           _b0;
+    float           _b1;
+    float           _b2;
+    float           _delay_element_1;        // buffered sample -1
+    float           _delay_element_2;        // buffered sample -2
+};
+
+} // namespace math

src/lib/mathlib/math/filter/module.mk

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# filter library
+#
+SRCS		 = LowPassFilter2p.cpp
+
+#
+# In order to include .config we first have to save off the
+# current makefile name, since app.mk needs it.
+#
+APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))

src/lib/mathlib/math/generic/Matrix.cpp

@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.cpp
+ *
+ * matrix code
+ */
+
+#include "Matrix.hpp"

src/lib/mathlib/math/generic/Matrix.hpp

@@ -0,0 +1,437 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Matrix.h
+ *
+ * matrix code
+ */
+
+#pragma once
+
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "../Vector.hpp"
+#include "../Matrix.hpp"
+
+namespace math
+{
+
+class __EXPORT Matrix
+{
+public:
+	// constructor
+	Matrix(size_t rows, size_t cols) :
+		_rows(rows),
+		_cols(cols),
+		_data((float *)calloc(rows *cols, sizeof(float))) {
+	}
+	Matrix(size_t rows, size_t cols, const float *data) :
+		_rows(rows),
+		_cols(cols),
+		_data((float *)malloc(getSize())) {
+		memcpy(getData(), data, getSize());
+	}
+	// deconstructor
+	virtual ~Matrix() {
+		delete [] getData();
+	}
+	// copy constructor (deep)
+	Matrix(const Matrix &right) :
+		_rows(right.getRows()),
+		_cols(right.getCols()),
+		_data((float *)malloc(getSize())) {
+		memcpy(getData(), right.getData(),
+		       right.getSize());
+	}
+	// assignment
+	inline Matrix &operator=(const Matrix &right) {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == right.getRows());
+		ASSERT(getCols() == right.getCols());
+#endif
+
+		if (this != &right) {
+			memcpy(getData(), right.getData(),
+			       right.getSize());
+		}
+
+		return *this;
+	}
+	// element accessors
+	inline float &operator()(size_t i, size_t j) {
+#ifdef MATRIX_ASSERT
+		ASSERT(i < getRows());
+		ASSERT(j < getCols());
+#endif
+		return getData()[i * getCols() + j];
+	}
+	inline const float &operator()(size_t i, size_t j) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(i < getRows());
+		ASSERT(j < getCols());
+#endif
+		return getData()[i * getCols() + j];
+	}
+	// output
+	inline void print() const {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				float sig;
+				int exp;
+				float num = (*this)(i, j);
+				float2SigExp(num, sig, exp);
+				printf("%6.3fe%03.3d,", (double)sig, exp);
+			}
+
+			printf("\n");
+		}
+	}
+	// boolean ops
+	inline bool operator==(const Matrix &right) const {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				if (fabsf((*this)(i, j) - right(i, j)) > 1e-30f)
+					return false;
+			}
+		}
+
+		return true;
+	}
+	// scalar ops
+	inline Matrix operator+(const float &right) const {
+		Matrix result(getRows(), getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) + right;
+			}
+		}
+
+		return result;
+	}
+	inline Matrix operator-(const float &right) const {
+		Matrix result(getRows(), getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) - right;
+			}
+		}
+
+		return result;
+	}
+	inline Matrix operator*(const float &right) const {
+		Matrix result(getRows(), getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) * right;
+			}
+		}
+
+		return result;
+	}
+	inline Matrix operator/(const float &right) const {
+		Matrix result(getRows(), getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) / right;
+			}
+		}
+
+		return result;
+	}
+	// vector ops
+	inline Vector operator*(const Vector &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getCols() == right.getRows());
+#endif
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i) += (*this)(i, j) * right(j);
+			}
+		}
+
+		return result;
+	}
+	// matrix ops
+	inline Matrix operator+(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == right.getRows());
+		ASSERT(getCols() == right.getCols());
+#endif
+		Matrix result(getRows(), getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) + right(i, j);
+			}
+		}
+
+		return result;
+	}
+	inline Matrix operator-(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == right.getRows());
+		ASSERT(getCols() == right.getCols());
+#endif
+		Matrix result(getRows(), getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(i, j) = (*this)(i, j) - right(i, j);
+			}
+		}
+
+		return result;
+	}
+	inline Matrix operator*(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getCols() == right.getRows());
+#endif
+		Matrix result(getRows(), right.getCols());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < right.getCols(); j++) {
+				for (size_t k = 0; k < right.getRows(); k++) {
+					result(i, j) += (*this)(i, k) * right(k, j);
+				}
+			}
+		}
+
+		return result;
+	}
+	inline Matrix operator/(const Matrix &right) const {
+#ifdef MATRIX_ASSERT
+		ASSERT(right.getRows() == right.getCols());
+		ASSERT(getCols() == right.getCols());
+#endif
+		return (*this) * right.inverse();
+	}
+	// other functions
+	inline Matrix transpose() const {
+		Matrix result(getCols(), getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				result(j, i) = (*this)(i, j);
+			}
+		}
+
+		return result;
+	}
+	inline void swapRows(size_t a, size_t b) {
+		if (a == b) return;
+
+		for (size_t j = 0; j < getCols(); j++) {
+			float tmp = (*this)(a, j);
+			(*this)(a, j) = (*this)(b, j);
+			(*this)(b, j) = tmp;
+		}
+	}
+	inline void swapCols(size_t a, size_t b) {
+		if (a == b) return;
+
+		for (size_t i = 0; i < getRows(); i++) {
+			float tmp = (*this)(i, a);
+			(*this)(i, a) = (*this)(i, b);
+			(*this)(i, b) = tmp;
+		}
+	}
+	/**
+	 * inverse based on LU factorization with partial pivotting
+	 */
+	Matrix inverse() const {
+#ifdef MATRIX_ASSERT
+		ASSERT(getRows() == getCols());
+#endif
+		size_t N = getRows();
+		Matrix L = identity(N);
+		const Matrix &A = (*this);
+		Matrix U = A;
+		Matrix P = identity(N);
+
+		//printf("A:\n"); A.print();
+
+		// for all diagonal elements
+		for (size_t n = 0; n < N; n++) {
+
+			// if diagonal is zero, swap with row below
+			if (fabsf(U(n, n)) < 1e-8f) {
+				//printf("trying pivot for row %d\n",n);
+				for (size_t i = 0; i < N; i++) {
+					if (i == n) continue;
+
+					//printf("\ttrying row %d\n",i);
+					if (fabsf(U(i, n)) > 1e-8f) {
+						//printf("swapped %d\n",i);
+						U.swapRows(i, n);
+						P.swapRows(i, n);
+					}
+				}
+			}
+
+#ifdef MATRIX_ASSERT
+			//printf("A:\n"); A.print();
+			//printf("U:\n"); U.print();
+			//printf("P:\n"); P.print();
+			//fflush(stdout);
+			ASSERT(fabsf(U(n, n)) > 1e-8f);
+#endif
+
+			// failsafe, return zero matrix
+			if (fabsf(U(n, n)) < 1e-8f) {
+				return Matrix::zero(n);
+			}
+
+			// for all rows below diagonal
+			for (size_t i = (n + 1); i < N; i++) {
+				L(i, n) = U(i, n) / U(n, n);
+
+				// add i-th row and n-th row
+				// multiplied by: -a(i,n)/a(n,n)
+				for (size_t k = n; k < N; k++) {
+					U(i, k) -= L(i, n) * U(n, k);
+				}
+			}
+		}
+
+		//printf("L:\n"); L.print();
+		//printf("U:\n"); U.print();
+
+		// solve LY=P*I for Y by forward subst
+		Matrix Y = P;
+
+		// for all columns of Y
+		for (size_t c = 0; c < N; c++) {
+			// for all rows of L
+			for (size_t i = 0; i < N; i++) {
+				// for all columns of L
+				for (size_t j = 0; j < i; j++) {
+					// for all existing y
+					// subtract the component they
+					// contribute to the solution
+					Y(i, c) -= L(i, j) * Y(j, c);
+				}
+
+				// divide by the factor
+				// on current
+				// term to be solved
+				// Y(i,c) /= L(i,i);
+				// but L(i,i) = 1.0
+			}
+		}
+
+		//printf("Y:\n"); Y.print();
+
+		// solve Ux=y for x by back subst
+		Matrix X = Y;
+
+		// for all columns of X
+		for (size_t c = 0; c < N; c++) {
+			// for all rows of U
+			for (size_t k = 0; k < N; k++) {
+				// have to go in reverse order
+				size_t i = N - 1 - k;
+
+				// for all columns of U
+				for (size_t j = i + 1; j < N; j++) {
+					// for all existing x
+					// subtract the component they
+					// contribute to the solution
+					X(i, c) -= U(i, j) * X(j, c);
+				}
+
+				// divide by the factor
+				// on current
+				// term to be solved
+				X(i, c) /= U(i, i);
+			}
+		}
+
+		//printf("X:\n"); X.print();
+		return X;
+	}
+	inline void setAll(const float &val) {
+		for (size_t i = 0; i < getRows(); i++) {
+			for (size_t j = 0; j < getCols(); j++) {
+				(*this)(i, j) = val;
+			}
+		}
+	}
+	inline void set(const float *data) {
+		memcpy(getData(), data, getSize());
+	}
+	inline size_t getRows() const { return _rows; }
+	inline size_t getCols() const { return _cols; }
+	inline static Matrix identity(size_t size) {
+		Matrix result(size, size);
+
+		for (size_t i = 0; i < size; i++) {
+			result(i, i) = 1.0f;
+		}
+
+		return result;
+	}
+	inline static Matrix zero(size_t size) {
+		Matrix result(size, size);
+		result.setAll(0.0f);
+		return result;
+	}
+	inline static Matrix zero(size_t m, size_t n) {
+		Matrix result(m, n);
+		result.setAll(0.0f);
+		return result;
+	}
+protected:
+	inline size_t getSize() const { return sizeof(float) * getRows() * getCols(); }
+	inline float *getData() { return _data; }
+	inline const float *getData() const { return _data; }
+	inline void setData(float *data) { _data = data; }
+private:
+	size_t _rows;
+	size_t _cols;
+	float *_data;
+};
+
+} // namespace math

src/lib/mathlib/math/generic/Vector.cpp

@@ -0,0 +1,40 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.cpp
+ *
+ * math vector
+ */
+
+#include "Vector.hpp"

src/lib/mathlib/math/generic/Vector.hpp

@@ -0,0 +1,245 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector.h
+ *
+ * math vector
+ */
+
+#pragma once
+
+#include <inttypes.h>
+#include <assert.h>
+#include <stdlib.h>
+#include <string.h>
+#include <stdio.h>
+#include <math.h>
+
+#include "../Vector.hpp"
+
+namespace math
+{
+
+class __EXPORT Vector
+{
+public:
+	// constructor
+	Vector(size_t rows) :
+		_rows(rows),
+		_data((float *)calloc(rows, sizeof(float))) {
+	}
+	Vector(size_t rows, const float *data) :
+		_rows(rows),
+		_data((float *)malloc(getSize())) {
+		memcpy(getData(), data, getSize());
+	}
+	// deconstructor
+	virtual ~Vector() {
+		delete [] getData();
+	}
+	// copy constructor (deep)
+	Vector(const Vector &right) :
+		_rows(right.getRows()),
+		_data((float *)malloc(getSize())) {
+		memcpy(getData(), right.getData(),
+		       right.getSize());
+	}
+	// assignment
+	inline Vector &operator=(const Vector &right) {
+#ifdef VECTOR_ASSERT
+		ASSERT(getRows() == right.getRows());
+#endif
+
+		if (this != &right) {
+			memcpy(getData(), right.getData(),
+			       right.getSize());
+		}
+
+		return *this;
+	}
+	// element accessors
+	inline float &operator()(size_t i) {
+#ifdef VECTOR_ASSERT
+		ASSERT(i < getRows());
+#endif
+		return getData()[i];
+	}
+	inline const float &operator()(size_t i) const {
+#ifdef VECTOR_ASSERT
+		ASSERT(i < getRows());
+#endif
+		return getData()[i];
+	}
+	// output
+	inline void print() const {
+		for (size_t i = 0; i < getRows(); i++) {
+			float sig;
+			int exp;
+			float num = (*this)(i);
+			float2SigExp(num, sig, exp);
+			printf("%6.3fe%03.3d,", (double)sig, exp);
+		}
+
+		printf("\n");
+	}
+	// boolean ops
+	inline bool operator==(const Vector &right) const {
+		for (size_t i = 0; i < getRows(); i++) {
+			if (fabsf(((*this)(i) - right(i))) > 1e-30f)
+				return false;
+		}
+
+		return true;
+	}
+	// scalar ops
+	inline Vector operator+(const float &right) const {
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) + right;
+		}
+
+		return result;
+	}
+	inline Vector operator-(const float &right) const {
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) - right;
+		}
+
+		return result;
+	}
+	inline Vector operator*(const float &right) const {
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) * right;
+		}
+
+		return result;
+	}
+	inline Vector operator/(const float &right) const {
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) / right;
+		}
+
+		return result;
+	}
+	// vector ops
+	inline Vector operator+(const Vector &right) const {
+#ifdef VECTOR_ASSERT
+		ASSERT(getRows() == right.getRows());
+#endif
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) + right(i);
+		}
+
+		return result;
+	}
+	inline Vector operator-(const Vector &right) const {
+#ifdef VECTOR_ASSERT
+		ASSERT(getRows() == right.getRows());
+#endif
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = (*this)(i) - right(i);
+		}
+
+		return result;
+	}
+	inline Vector operator-(void) const {
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = -((*this)(i));
+		}
+
+		return result;
+	}
+	// other functions
+	inline float dot(const Vector &right) const {
+		float result = 0;
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result += (*this)(i) * (*this)(i);
+		}
+
+		return result;
+	}
+	inline float norm() const {
+		return sqrtf(dot(*this));
+	}
+	inline float length() const {
+		return norm();
+	}
+	inline Vector unit() const {
+		return (*this) / norm();
+	}
+	inline Vector normalized() const {
+		return unit();
+	}
+	inline void normalize() {
+		(*this) = (*this) / norm();
+	}
+	inline static Vector zero(size_t rows) {
+		Vector result(rows);
+		// calloc returns zeroed memory
+		return result;
+	}
+	inline void setAll(const float &val) {
+		for (size_t i = 0; i < getRows(); i++) {
+			(*this)(i) = val;
+		}
+	}
+	inline void set(const float *data) {
+		memcpy(getData(), data, getSize());
+	}
+	inline size_t getRows() const { return _rows; }
+protected:
+	inline size_t getSize() const { return sizeof(float) * getRows(); }
+	inline float *getData() { return _data; }
+	inline const float *getData() const { return _data; }
+	inline void setData(float *data) { _data = data; }
+private:
+	size_t _rows;
+	float *_data;
+};
+
+} // math

src/lib/mathlib/math/test/test.cpp

@@ -0,0 +1,94 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test.cpp
+ *
+ * Test library code
+ */
+
+#include <stdio.h>
+#include <math.h>
+#include <stdlib.h>
+
+#include "test.hpp"
+
+bool __EXPORT equal(float a, float b, float epsilon)
+{
+	float diff = fabsf(a - b);
+
+	if (diff > epsilon) {
+		printf("not equal ->\n\ta: %12.8f\n\tb: %12.8f\n", double(a), double(b));
+		return false;
+
+	} else return true;
+}
+
+void __EXPORT float2SigExp(
+	const float &num,
+	float &sig,
+	int &exp)
+{
+	if (isnan(num) || isinf(num)) {
+		sig = 0.0f;
+		exp = -99;
+		return;
+	}
+
+	if (fabsf(num) < 1.0e-38f) {
+		sig = 0;
+		exp = 0;
+		return;
+	}
+
+	exp = log10f(fabsf(num));
+
+	if (exp > 0) {
+		exp = ceil(exp);
+
+	} else {
+		exp = floor(exp);
+	}
+
+	sig = num;
+
+	// cheap power since it is integer
+	if (exp > 0) {
+		for (int i = 0; i < abs(exp); i++) sig /= 10;
+
+	} else {
+		for (int i = 0; i < abs(exp); i++) sig *= 10;
+	}
+}
+
+

src/lib/mathlib/math/test/test.hpp

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test.hpp
+ *
+ * Controller library code
+ */
+
+#pragma once
+
+//#include <assert.h>
+//#include <time.h>
+//#include <stdlib.h>
+
+bool equal(float a, float b, float eps = 1e-5);
+void float2SigExp(
+	const float &num,
+	float &sig,
+	int &exp);

src/lib/mathlib/math/test_math.sce

@@ -0,0 +1,63 @@
+clc
+clear
+function out = float_truncate(in, digits)
+    out = round(in*10^digits)
+    out = out/10^digits
+endfunction
+
+phi = 0.1
+theta = 0.2
+psi = 0.3
+
+cosPhi = cos(phi)
+cosPhi_2 = cos(phi/2)
+sinPhi = sin(phi)
+sinPhi_2 = sin(phi/2)
+
+cosTheta = cos(theta)
+cosTheta_2 = cos(theta/2)
+sinTheta = sin(theta)
+sinTheta_2 = sin(theta/2)
+
+cosPsi = cos(psi)
+cosPsi_2 = cos(psi/2)
+sinPsi = sin(psi)
+sinPsi_2 = sin(psi/2)
+
+C_nb = [cosTheta*cosPsi, -cosPhi*sinPsi + sinPhi*sinTheta*cosPsi, sinPhi*sinPsi + cosPhi*sinTheta*cosPsi;
+        cosTheta*sinPsi, cosPhi*cosPsi + sinPhi*sinTheta*sinPsi, -sinPhi*cosPsi + cosPhi*sinTheta*sinPsi;
+        -sinTheta, sinPhi*cosTheta, cosPhi*cosTheta]
+    
+disp(C_nb)
+//C_nb = float_truncate(C_nb,3)
+//disp(C_nb)
+
+theta = asin(-C_nb(3,1))
+phi = atan(C_nb(3,2), C_nb(3,3))
+psi = atan(C_nb(2,1), C_nb(1,1))
+printf('phi %f\n', phi)
+printf('theta %f\n', theta)
+printf('psi %f\n', psi)
+
+q = [cosPhi_2*cosTheta_2*cosPsi_2 + sinPhi_2*sinTheta_2*sinPsi_2;
+     sinPhi_2*cosTheta_2*cosPsi_2 - cosPhi_2*sinTheta_2*sinPsi_2;
+     cosPhi_2*sinTheta_2*cosPsi_2 + sinPhi_2*cosTheta_2*sinPsi_2;
+     cosPhi_2*cosTheta_2*sinPsi_2 - sinPhi_2*sinTheta_2*cosPsi_2]
+     
+//q = float_truncate(q,3)
+
+a = q(1)
+b = q(2)
+c = q(3)
+d = q(4)
+printf('q: %f %f %f %f\n', a, b, c, d)
+a2 = a*a
+b2 = b*b
+c2 = c*c
+d2 = d*d
+
+C2_nb = [a2 + b2 - c2 - d2, 2*(b*c - a*d), 2*(b*d + a*c);
+         2*(b*c + a*d), a2 - b2 + c2 - d2, 2*(c*d - a*b);
+         2*(b*d - a*c), 2*(c*d + a*b), a2 - b2 - c2 + d2]
+
+disp(C2_nb)

src/lib/mathlib/mathlib.h

@@ -0,0 +1,59 @@
+/****************************************************************************
+ *
+ *   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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file mathlib.h
+ *
+ * Common header for mathlib exports.
+ */
+
+#ifdef __cplusplus
+
+#pragma once
+
+#include "math/Dcm.hpp"
+#include "math/EulerAngles.hpp"
+#include "math/Matrix.hpp"
+#include "math/Quaternion.hpp"
+#include "math/Vector.hpp"
+#include "math/Vector3.hpp"
+#include "math/Vector2f.hpp"
+#include "math/Limits.hpp"
+
+#endif
+
+#ifdef CONFIG_ARCH_ARM
+
+#include "CMSIS/Include/arm_math.h"
+
+#endif

src/lib/mathlib/module.mk

@@ -0,0 +1,61 @@
+############################################################################
+#
+#   Copyright (c) 2012, 2013 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.
+#
+############################################################################
+
+#
+# Math library
+#
+SRCS		 = math/test/test.cpp \
+		   math/Vector.cpp \
+		   math/Vector2f.cpp \
+		   math/Vector3.cpp \
+		   math/EulerAngles.cpp \
+		   math/Quaternion.cpp \
+		   math/Dcm.cpp \
+		   math/Matrix.cpp \
+		   math/Limits.cpp
+
+#
+# In order to include .config we first have to save off the
+# current makefile name, since app.mk needs it.
+#
+APP_MAKEFILE	:= $(lastword $(MAKEFILE_LIST))
+
+ifeq ($(CONFIG_ARCH_CORTEXM4)$(CONFIG_ARCH_FPU),yy)
+INCLUDE_DIRS	+= math/arm
+SRCS		+= math/arm/Vector.cpp \
+		   math/arm/Matrix.cpp
+else
+#INCLUDE_DIRS	+= math/generic
+#SRCS		+= math/generic/Vector.cpp \
+#		   math/generic/Matrix.cpp
+endif