Merge branch 'master' of github.com:PX4/Firmware into pca8574

12 files changed, 265 insertions, 5 deletions

diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
index 9584924cc..0a909d02f 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp
@@ -63,11 +63,22 @@ ECL_PitchController::ECL_PitchController() :
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f)
 {
+	perf_alloc(PC_COUNT, "fw att control pitch nonfinite input");
 }
 
-float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
+ECL_PitchController::~ECL_PitchController()
 {
+	perf_free(_nonfinite_input_perf);
+}
 
+float ECL_PitchController::control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed)
+{
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(pitch_setpoint) && isfinite(roll) && isfinite(pitch) && isfinite(airspeed))) {
+		perf_count(_nonfinite_input_perf);
+		warnx("not controlling pitch");
+		return _rate_setpoint;
+	}
 
 	/* flying inverted (wings upside down) ? */
 	bool inverted = false;
@@ -123,6 +134,14 @@ float ECL_PitchController::control_bodyrate(float roll, float pitch,
 		float yaw_rate_setpoint,
 		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
+				isfinite(yaw_rate_setpoint) && isfinite(airspeed_min) &&
+				isfinite(airspeed_max) && isfinite(scaler))) {
+		perf_count(_nonfinite_input_perf);
+		return math::constrain(_last_output, -1.0f, 1.0f);
+	}
+
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
diff --git a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
index 30a82a86a..39b9f9d03 100644
--- a/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_pitch_controller.h
@@ -51,12 +51,15 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <systemlib/perf_counter.h>
 
 class __EXPORT ECL_PitchController //XXX: create controller superclass
 {
 public:
 	ECL_PitchController();
 
+	~ECL_PitchController();
+
 	float control_attitude(float pitch_setpoint, float roll, float pitch, float airspeed);
 
 
@@ -126,6 +129,7 @@ private:
 	float _rate_error;
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
+	perf_counter_t _nonfinite_input_perf;
 };
 
 #endif // ECL_PITCH_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
index 2e86c72dc..82903ef5a 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.cpp
@@ -61,10 +61,21 @@ ECL_RollController::ECL_RollController() :
 	_rate_setpoint(0.0f),
 	_bodyrate_setpoint(0.0f)
 {
+	perf_alloc(PC_COUNT, "fw att control roll nonfinite input");
+}
+
+ECL_RollController::~ECL_RollController()
+{
+	perf_free(_nonfinite_input_perf);
 }
 
 float ECL_RollController::control_attitude(float roll_setpoint, float roll)
 {
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(roll_setpoint) && isfinite(roll))) {
+		perf_count(_nonfinite_input_perf);
+		return _rate_setpoint;
+	}
 
 	/* Calculate error */
 	float roll_error = roll_setpoint - roll;
@@ -86,6 +97,14 @@ float ECL_RollController::control_bodyrate(float pitch,
 		float yaw_rate_setpoint,
 		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(pitch) && isfinite(roll_rate) && isfinite(yaw_rate) && isfinite(yaw_rate_setpoint) &&
+			isfinite(airspeed_min) && isfinite(airspeed_max) &&
+			isfinite(scaler))) {
+		perf_count(_nonfinite_input_perf);
+		return math::constrain(_last_output, -1.0f, 1.0f);
+	}
+
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
@@ -122,8 +141,8 @@ float ECL_RollController::control_bodyrate(float pitch,
 		float id = _rate_error * dt;
 
 		/*
-		 * anti-windup: do not allow integrator to increase if actuator is at limit
-		 */
+		* anti-windup: do not allow integrator to increase if actuator is at limit
+		*/
 		if (_last_output < -1.0f) {
 			/* only allow motion to center: increase value */
 			id = math::max(id, 0.0f);
diff --git a/src/lib/ecl/attitude_fw/ecl_roll_controller.h b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
index 92c64b95f..0799dbe03 100644
--- a/src/lib/ecl/attitude_fw/ecl_roll_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_roll_controller.h
@@ -51,12 +51,15 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <systemlib/perf_counter.h>
 
 class __EXPORT ECL_RollController //XXX: create controller superclass
 {
 public:
 	ECL_RollController();
 
+	~ECL_RollController();
+
 	float control_attitude(float roll_setpoint, float roll);
 
 	float control_bodyrate(float pitch,
@@ -117,6 +120,7 @@ private:
 	float _rate_error;
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
+	perf_counter_t _nonfinite_input_perf;
 };
 
 #endif // ECL_ROLL_CONTROLLER_H
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 255776765..e53ffc644 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -60,12 +60,25 @@ ECL_YawController::ECL_YawController() :
 	_bodyrate_setpoint(0.0f),
 	_coordinated_min_speed(1.0f)
 {
+	perf_alloc(PC_COUNT, "fw att control yaw nonfinite input");
+}
+
+ECL_YawController::~ECL_YawController()
+{
+	perf_free(_nonfinite_input_perf);
 }
 
 float ECL_YawController::control_attitude(float roll, float pitch,
 		float speed_body_u, float speed_body_v, float speed_body_w,
 		float roll_rate_setpoint, float pitch_rate_setpoint)
 {
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(roll) && isfinite(pitch) && isfinite(speed_body_u) && isfinite(speed_body_v) &&
+				isfinite(speed_body_w) && isfinite(roll_rate_setpoint) &&
+				isfinite(pitch_rate_setpoint))) {
+		perf_count(_nonfinite_input_perf);
+		return _rate_setpoint;
+	}
 //	static int counter = 0;
 	/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
 	_rate_setpoint = 0.0f;
@@ -103,6 +116,13 @@ float ECL_YawController::control_bodyrate(float roll, float pitch,
 		float pitch_rate_setpoint,
 		float airspeed_min, float airspeed_max, float airspeed, float scaler, bool lock_integrator)
 {
+	/* Do not calculate control signal with bad inputs */
+	if (!(isfinite(roll) && isfinite(pitch) && isfinite(pitch_rate) && isfinite(yaw_rate) &&
+				isfinite(pitch_rate_setpoint) && isfinite(airspeed_min) &&
+				isfinite(airspeed_max) && isfinite(scaler))) {
+		perf_count(_nonfinite_input_perf);
+		return math::constrain(_last_output, -1.0f, 1.0f);
+	}
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index 03f3202d0..a360c14b8 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -50,12 +50,15 @@
 
 #include <stdbool.h>
 #include <stdint.h>
+#include <systemlib/perf_counter.h>
 
 class __EXPORT ECL_YawController //XXX: create controller superclass
 {
 public:
 	ECL_YawController();
 
+	~ECL_YawController();
+
 	float control_attitude(float roll, float pitch,
 			float speed_body_u, float speed_body_v, float speed_body_w,
 			float roll_rate_setpoint, float pitch_rate_setpoint);
@@ -118,6 +121,7 @@ private:
 	float _rate_setpoint;
 	float _bodyrate_setpoint;
 	float _coordinated_min_speed;
+	perf_counter_t _nonfinite_input_perf;
 
 };
 
diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h
index 0a3f85d97..e2f3da6f8 100644
--- a/src/lib/geo/geo.h
+++ b/src/lib/geo/geo.h
@@ -50,6 +50,8 @@
 
 __BEGIN_DECLS
 
+#include "geo/geo_mag_declination.h"
+
 #include <stdbool.h>
 
 #define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c
new file mode 100644
index 000000000..09eac38f4
--- /dev/null
+++ b/src/lib/geo/geo_mag_declination.c
@@ -0,0 +1,136 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 MAV GEO Library (MAVGEO). 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 MAVGEO 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_mag_declination.c
+*
+* Calculation / lookup table for earth magnetic field declination.
+* 
+* Lookup table from Scott Ferguson <scottfromscott@gmail.com>
+*
+* XXX Lookup table currently too coarse in resolution (only full degrees)
+* and lat/lon res - needs extension medium term.
+*
+*/
+
+#include <geo/geo.h>
+
+/** set this always to the sampling in degrees for the table below */
+#define SAMPLING_RES		10.0f
+#define SAMPLING_MIN_LAT	-60.0f
+#define SAMPLING_MAX_LAT	60.0f
+#define SAMPLING_MIN_LON	-180.0f
+#define SAMPLING_MAX_LON	180.0f
+
+static const int8_t declination_table[13][37] = \
+{
+	46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \
+	-66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \
+	-3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \
+	29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \
+	-40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \
+	8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \
+	10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \
+	-10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \
+	-14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \
+	9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \
+	7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \
+	0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \
+	0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \
+	0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \
+	-16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \
+	12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \
+	0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \
+	13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3
+};
+
+static float get_lookup_table_val(unsigned lat, unsigned lon);
+
+__EXPORT float get_mag_declination(float lat, float lon)
+{
+	/*
+	 * If the values exceed valid ranges, return zero as default
+	 * as we have no way of knowing what the closest real value
+	 * would be.
+	 */
+	if (lat < -90.0f || lat > 90.0f ||
+	    lon < -180.0f || lon > 180.0f) {
+		return 0.0f;
+	}
+
+	/* round down to nearest sampling resolution */
+	int min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES;
+	int min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES;
+
+	/* for the rare case of hitting the bounds exactly
+	 * the rounding logic wouldn't fit, so enforce it.
+	 */
+
+	/* limit to table bounds - required for maxima even when table spans full globe range */
+	if (lat <= SAMPLING_MIN_LAT) {
+		min_lat = SAMPLING_MIN_LAT;
+	}
+
+	if (lat >= SAMPLING_MAX_LAT) {
+		min_lat = (int)(lat / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
+	}
+
+	if (lon <= SAMPLING_MIN_LON) {
+		min_lon = SAMPLING_MIN_LON;
+	}
+
+	if (lon >= SAMPLING_MAX_LON) {
+		min_lon = (int)(lon / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES;
+	}
+
+	/* find index of nearest low sampling point */
+	unsigned min_lat_index = (-(SAMPLING_MIN_LAT) + min_lat)  / SAMPLING_RES;
+	unsigned min_lon_index = (-(SAMPLING_MIN_LON) + min_lon) / SAMPLING_RES;
+
+	float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index);
+	float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1);
+	float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1);
+	float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index);
+
+	/* perform bilinear interpolation on the four grid corners */
+
+	float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw;
+	float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw;
+
+	return ((lat - min_lat) / SAMPLING_RES) * (declination_max - declination_min) + declination_min;
+}
+
+float get_lookup_table_val(unsigned lat_index, unsigned lon_index)
+{
+	return declination_table[lat_index][lon_index];
+}
\ No newline at end of file
diff --git a/src/lib/geo/geo_mag_declination.h b/src/lib/geo/geo_mag_declination.h
new file mode 100644
index 000000000..0ac062d6d
--- /dev/null
+++ b/src/lib/geo/geo_mag_declination.h
@@ -0,0 +1,47 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 MAV GEO Library (MAVGEO). 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 MAVGEO 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_mag_declination.h
+*
+* Calculation / lookup table for earth magnetic field declination.
+*
+*/
+
+#pragma once
+
+__BEGIN_DECLS
+
+__EXPORT float get_mag_declination(float lat, float lon);
+
+__END_DECLS
diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk
index 30a2dc99f..9500a2bcc 100644
--- a/src/lib/geo/module.mk
+++ b/src/lib/geo/module.mk
@@ -35,4 +35,5 @@
 # Geo library
 #
 
-SRCS		 = geo.c
+SRCS		=	geo.c \
+			geo_mag_declination.c
diff --git a/src/lib/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h
index 6f66f9ee3..61d3a3b61 100644
--- a/src/lib/mathlib/CMSIS/Include/arm_math.h
+++ b/src/lib/mathlib/CMSIS/Include/arm_math.h
@@ -5193,7 +5193,7 @@ void arm_rfft_fast_f32(
     *pIa = Ialpha;

 

     /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */

-    *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;

+    *pIb = (float32_t)-0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta;

 

   }

 

diff --git a/src/lib/version/version.h b/src/lib/version/version.h
index af733aaf0..d8ccb6774 100644
--- a/src/lib/version/version.h
+++ b/src/lib/version/version.h
@@ -59,4 +59,8 @@
 #define	HW_ARCH "PX4FMU_V2"
 #endif
 
+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#define	HW_ARCH "AEROCORE"
+#endif
+
 #endif /* VERSION_H_ */