From 83eb81251c8405c96aaa72fc1634d15a5b54af29 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 1 Nov 2014 11:22:18 +0100
Subject: NSH term: Only time out if no arming information is available, if
 arming information is available abort if unconnected on arming

---
 src/systemcmds/nshterm/nshterm.c | 27 +++++++++++++++++++++++++--
 1 file changed, 25 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/systemcmds/nshterm/nshterm.c b/src/systemcmds/nshterm/nshterm.c
index f06c49552..edeb5c624 100644
--- a/src/systemcmds/nshterm/nshterm.c
+++ b/src/systemcmds/nshterm/nshterm.c
@@ -51,6 +51,8 @@
 #include <fcntl.h>
 #include <systemlib/err.h>
 
+#include <uORB/topics/actuator_armed.h>
+
 __EXPORT int nshterm_main(int argc, char *argv[]);
 
 int
@@ -62,9 +64,30 @@ nshterm_main(int argc, char *argv[])
     }
     unsigned retries = 0;
     int fd = -1;
+    int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
+    struct actuator_armed_s armed;
+    /* we assume the system does not provide arming status feedback */
+    bool armed_updated = false;
+
+    /* try the first 30 seconds or if arming system is ready */
+    while ((retries < 300) || armed_updated) {
+
+        /* abort if an arming topic is published and system is armed */
+        bool updated = false;
+        if (orb_check(armed_fd, &updated)) {
+            /* the system is now providing arming status feedback.
+             * instead of timing out, we resort to abort bringing
+             * up the terminal.
+             */
+            armed_updated = true;
+            orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);
+
+            if (armed.armed) {
+                /* this is not an error, but we are done */
+                exit(0);
+            }
+        }
 
-    /* try the first 30 seconds */
-    while (retries < 300) {
         /* the retries are to cope with the behaviour of /dev/ttyACM0 */
         /* which may not be ready immediately. */
         fd = open(argv[1], O_RDWR);
-- 
cgit v1.2.3


From c442f820dd89aad9fac394ce56c07b10cfcacf73 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 1 Nov 2014 16:54:11 +0100
Subject: Encode RC type in RSSI field for GCS

---
 src/modules/mavlink/mavlink_messages.cpp | 43 +++++++++++++++++++++++++++++++-
 1 file changed, 42 insertions(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index cccb698bf..87858690f 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1717,7 +1717,48 @@ protected:
 			msg.chan16_raw = (rc.channel_count > 15) ? rc.values[15] : UINT16_MAX;
 			msg.chan17_raw = (rc.channel_count > 16) ? rc.values[16] : UINT16_MAX;
 			msg.chan18_raw = (rc.channel_count > 17) ? rc.values[17] : UINT16_MAX;
-			msg.rssi = rc.rssi;
+
+			/* RSSI has a max value of 100, and when Spektrum or S.BUS are
+			 * available, the RSSI field is invalid, as they do not provide
+			 * an RSSI measurement. Use an out of band magic value to signal
+			 * these digital ports. XXX revise MAVLink spec to address this.
+			 * One option would be to use the top bit to toggle between RSSI
+			 * and input source mode.
+			 *
+			 * Full RSSI field: 0b 1 111 1111
+			 *
+			 *                     ^ If bit is set, RSSI encodes type + RSSI
+			 *
+			 *                       ^ These three bits encode a total of 8
+			 *                         digital RC input types.
+			 *                         0: PPM, 1: SBUS, 2: Spektrum, 2: ST24
+			 *                           ^ These four bits encode a total of
+			 *                             16 RSSI levels. 15 = full, 0 = no signal
+			 *
+			 */
+
+			/* Initialize RSSI with the special mode level flag */
+			msg.rssi = (1 << 7);
+
+			/* Set RSSI */
+			msg.rssi |= (rc.rssi <= 100) ? ((rc.rssi / 7) + 1) : 15;
+
+			switch (rc.input_source) {
+				case RC_INPUT_SOURCE_PX4FMU_PPM:
+				/* fallthrough */
+				case RC_INPUT_SOURCE_PX4IO_PPM:
+					msg.rssi |= (0 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_SPEKTRUM:
+					msg.rssi |= (1 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_SBUS:
+					msg.rssi |= (2 << 4);
+					break;
+				case RC_INPUT_SOURCE_PX4IO_ST24:
+					msg.rssi |= (3 << 4);
+					break;
+			}
 
 			_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
 		}
-- 
cgit v1.2.3


From 412ddde5dca055ba090ed2e18db2dc77b6af93a4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 1 Nov 2014 16:58:12 +0100
Subject: Force RSSI to zero if RC is lost

---
 src/modules/mavlink/mavlink_messages.cpp | 5 +++++
 1 file changed, 5 insertions(+)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 87858690f..a2b3cc62e 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1760,6 +1760,11 @@ protected:
 					break;
 			}
 
+			if (rc.rc_lost) {
+				/* RSSI is by definition zero */
+				msg.rssi = 0;
+			}
+
 			_mavlink->send_message(MAVLINK_MSG_ID_RC_CHANNELS, &msg);
 		}
 	}
-- 
cgit v1.2.3


From 58f36714f808c9d06527da1952d348201c4f7e72 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Thu, 14 Aug 2014 15:43:27 +0200
Subject: UAVCAN: allow to arm single ESCs

---
 src/modules/uavcan/actuators/esc.cpp | 26 +++++++++++++++++++++-----
 src/modules/uavcan/actuators/esc.hpp |  9 ++++++++-
 src/modules/uavcan/uavcan_main.cpp   |  2 +-
 3 files changed, 30 insertions(+), 7 deletions(-)

(limited to 'src')

diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index 1990651ef..fbd4f0bcd 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -40,6 +40,9 @@
 #include "esc.hpp"
 #include <systemlib/err.h>
 
+
+#define MOTOR_BIT(x) (1<<(x))
+
 UavcanEscController::UavcanEscController(uavcan::INode &node) :
 	_node(node),
 	_uavcan_pub_raw_cmd(node),
@@ -95,9 +98,8 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 
 	static const int cmd_max = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::RawValueType::max();
 
-	if (_armed) {
-		for (unsigned i = 0; i < num_outputs; i++) {
-
+	for (unsigned i = 0; i < num_outputs; i++) {
+		if (_armed_mask & MOTOR_BIT(i)) {
 			float scaled = (outputs[i] + 1.0F) * 0.5F * cmd_max;
 			if (scaled < 1.0F) {
 				scaled = 1.0F;  // Since we're armed, we don't want to stop it completely
@@ -112,6 +114,9 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 
 			_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
 		}
+		else {
+			msg.cmd.push_back(static_cast<unsigned>(0));
+		}
 	}
 
 	/*
@@ -121,9 +126,20 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 	(void)_uavcan_pub_raw_cmd.broadcast(msg);
 }
 
-void UavcanEscController::arm_esc(bool arm)
+void UavcanEscController::arm_all_escs(bool arm)
+{
+	if (arm)
+		_armed_mask = -1;
+	else
+		_armed_mask = 0;
+}
+
+void UavcanEscController::arm_single_esc(int num, bool arm)
 {
-	_armed = arm;
+	if (arm)
+		_armed_mask = MOTOR_BIT(num);
+	else
+		_armed_mask = 0;
 }
 
 void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index f4a3877e6..ff3ecfb21 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -61,7 +61,8 @@ public:
 
 	void update_outputs(float *outputs, unsigned num_outputs);
 
-	void arm_esc(bool arm);
+	void arm_all_escs(bool arm);
+	void arm_single_esc(int num, bool arm);
 
 private:
 	/**
@@ -98,6 +99,12 @@ private:
 	uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder>	_uavcan_sub_status;
 	uavcan::TimerEventForwarder<TimerCbBinder>				_orb_timer;
 
+	/*
+	 * ESC states
+	 */
+	uint32_t 			_armed_mask = 0;
+	uavcan::equipment::esc::Status	_states[MAX_ESCS];
+
 	/*
 	 * Perf counters
 	 */
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index a8485ee44..59f0f4300 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -429,7 +429,7 @@ int
 UavcanNode::arm_actuators(bool arm)
 {
 	_is_armed = arm;
-	_esc_controller.arm_esc(arm);
+	_esc_controller.arm_all_escs(arm);
 	return OK;
 }
 
-- 
cgit v1.2.3


From a18460183cdfa590af29c2299acb30206b418cb1 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Mon, 3 Nov 2014 19:37:44 +0100
Subject: motor_test: cleanup

---
 src/modules/uORB/topics/test_motor.h   |  2 +-
 src/systemcmds/motor_test/motor_test.c | 23 ++++++++++++-----------
 2 files changed, 13 insertions(+), 12 deletions(-)

(limited to 'src')

diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h
index 491096934..c880fe099 100644
--- a/src/modules/uORB/topics/test_motor.h
+++ b/src/modules/uORB/topics/test_motor.h
@@ -57,7 +57,7 @@
 struct test_motor_s {
 	uint64_t 	timestamp;				/**< output timestamp in us since system boot */
 	unsigned 	motor_number;				/**< number of motor to spin */
-	float		value;					/**< output data, in natural output units */
+	float		value;					/**< output power, range [0..1]
 };
 
 /**
diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c
index 079f99674..a73f46626 100644
--- a/src/systemcmds/motor_test/motor_test.c
+++ b/src/systemcmds/motor_test/motor_test.c
@@ -54,7 +54,8 @@
 #include "systemlib/err.h"
 
 
-__EXPORT int	motor_test_main(int argc, char *argv[]);
+__EXPORT int motor_test_main(int argc, char *argv[]);
+
 static void motor_test(unsigned channel, float value);
 static void usage(const char *reason);
 
@@ -67,13 +68,13 @@ void motor_test(unsigned channel, float value)
 	_test_motor.timestamp = hrt_absolute_time();
 	_test_motor.value = value;
 
-        if (_test_motor_pub > 0) {
-	    /* publish armed state */
-            orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
-        } else {
-            /* advertise and publish */
-            _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
-        }
+    if (_test_motor_pub > 0) {
+    /* publish test state */
+        orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
+    } else {
+        /* advertise and publish */
+        _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
+    }
 }
 
 static void usage(const char *reason)
@@ -102,18 +103,18 @@ int motor_test_main(int argc, char *argv[])
 		switch (ch) {
 
 		case 'm':
-			/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
+			/* Read in motor number */
 			channel = strtoul(optarg, NULL, 0);
 			break;
 
 		case 'p':
-			/* Read in custom low value */
+			/* Read in power value */
 			lval = strtoul(optarg, NULL, 0);
 
 			if (lval > 100)
 				usage("value invalid");
 
-			value = (float)lval/100.f;
+			value = ((float)lval)/100.f;
 			break;
 		default:
 			usage(NULL);
-- 
cgit v1.2.3


From 0800fa4715994921b6a0d15cd2c44b9e51417117 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Mon, 3 Nov 2014 18:47:07 +0100
Subject: UAVCAN: implemented motor testing

---
 src/modules/uavcan/actuators/esc.hpp |  1 -
 src/modules/uavcan/uavcan_main.cpp   | 28 ++++++++++++++++++++++++----
 src/modules/uavcan/uavcan_main.hpp   |  5 +++++
 3 files changed, 29 insertions(+), 5 deletions(-)

(limited to 'src')

diff --git a/src/modules/uavcan/actuators/esc.hpp b/src/modules/uavcan/actuators/esc.hpp
index ff3ecfb21..12c035542 100644
--- a/src/modules/uavcan/actuators/esc.hpp
+++ b/src/modules/uavcan/actuators/esc.hpp
@@ -103,7 +103,6 @@ private:
 	 * ESC states
 	 */
 	uint32_t 			_armed_mask = 0;
-	uavcan::equipment::esc::Status	_states[MAX_ESCS];
 
 	/*
 	 * Perf counters
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 59f0f4300..653d4f98c 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -279,6 +279,7 @@ int UavcanNode::run()
 	_output_count = 2;
 
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+	_test_motor_sub = orb_subscribe(ORB_ID(test_motor));
 
 	actuator_outputs_s outputs;
 	memset(&outputs, 0, sizeof(outputs));
@@ -344,7 +345,14 @@ int UavcanNode::run()
 			}
 
 			// can we mix?
-			if (controls_updated && (_mixers != nullptr)) {
+			if (_test_in_progress) {
+				float outputs[NUM_ACTUATOR_OUTPUTS] = {};
+				outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
+
+				// Output to the bus
+				_esc_controller.update_outputs(outputs, NUM_ACTUATOR_OUTPUTS);
+			}
+			else if (controls_updated && (_mixers != nullptr)) {
 
 				// XXX one output group has 8 outputs max,
 				// but this driver could well serve multiple groups.
@@ -384,15 +392,27 @@ int UavcanNode::run()
 			}
 		}
 
-		// Check arming state
+		// Check motor test state
 		bool updated = false;
+		orb_check(_test_motor_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(test_motor), _test_motor_sub, &_test_motor);
+
+			// Update the test status and check that we're not locked down
+			_test_in_progress = (_test_motor.value > 0);
+			_esc_controller.arm_single_esc(_test_motor.motor_number, _test_in_progress);
+		}
+
+		// Check arming state
 		orb_check(_armed_sub, &updated);
 
 		if (updated) {
 			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
 
-			// Update the armed status and check that we're not locked down
-			bool set_armed = _armed.armed && !_armed.lockdown;
+			// Update the armed status and check that we're not locked down and motor 
+			// test is not running
+			bool set_armed = _armed.armed && !_armed.lockdown && !_test_in_progress;
 
 			arm_actuators(set_armed);
 		}
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
index be7db9741..274321f0d 100644
--- a/src/modules/uavcan/uavcan_main.hpp
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -41,6 +41,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/test_motor.h>
 
 #include "actuators/esc.hpp"
 #include "sensors/sensor_bridge.hpp"
@@ -103,6 +104,10 @@ private:
 	actuator_armed_s	_armed;				///< the arming request of the system
 	bool			_is_armed = false;		///< the arming status of the actuators on the bus
 
+	int			_test_motor_sub = -1;   ///< uORB subscription of the test_motor status
+	test_motor_s		_test_motor;
+	bool			_test_in_progress = false;
+
 	unsigned		_output_count = 0;		///< number of actuators currently available
 
 	static UavcanNode	*_instance;			///< singleton pointer
-- 
cgit v1.2.3


From 182c1c1d52ef370a42b99c825e3f45a03ecbcdc9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 5 Nov 2014 17:17:34 +0100
Subject: Fix RC mapping indices - 0 index induces issues right now

---
 src/drivers/px4io/px4io.cpp | 15 ++++++++++-----
 1 file changed, 10 insertions(+), 5 deletions(-)

(limited to 'src')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index fd9eb4170..c6543982e 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1247,28 +1247,33 @@ PX4IO::io_set_rc_config()
 	 */
 	param_get(param_find("RC_MAP_ROLL"), &ichan);
 
+	/* subtract one from 1-based index - this might be
+	 * a negative number now
+	 */
+	ichan -= 1;
+
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan - 1] = 0;
+		input_map[ichan] = 0;
 
 	param_get(param_find("RC_MAP_PITCH"), &ichan);
 
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan - 1] = 1;
+		input_map[ichan] = 1;
 
 	param_get(param_find("RC_MAP_YAW"), &ichan);
 
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan - 1] = 2;
+		input_map[ichan] = 2;
 
 	param_get(param_find("RC_MAP_THROTTLE"), &ichan);
 
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan - 1] = 3;
+		input_map[ichan] = 3;
 
 	param_get(param_find("RC_MAP_FLAPS"), &ichan);
 
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan - 1] = 4;
+		input_map[ichan] = 4;
 
 	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
 
-- 
cgit v1.2.3


From 23a33e31cd91d074f2d16d04b0cd7f2c867d9d4b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 5 Nov 2014 19:26:10 +0100
Subject: Add missing mode switch channel

---
 src/drivers/px4io/px4io.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index c6543982e..6d68d9f60 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1279,7 +1279,7 @@ PX4IO::io_set_rc_config()
 
 	if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
 		/* use out of normal bounds index to indicate special channel */
-		input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
+		input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
 	}
 
 	/*
-- 
cgit v1.2.3


From d907b030ee618b581e31964a34a61648e88793a3 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Wed, 5 Nov 2014 13:15:52 -0800
Subject: Initialize RSSI so it doesn't remain uninitialized
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit

Spektrum doesn’t support rssi so it is not set by st24_decode.
---
 src/modules/px4iofirmware/controls.c | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index ad60ee03e..3fd73fc60 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -41,6 +41,7 @@
 #include <stdbool.h>
 
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_rc_input.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/ppm_decode.h>
 #include <rc/st24.h>
@@ -91,6 +92,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 
 	for (unsigned i = 0; i < n_bytes; i++) {
 		/* set updated flag if one complete packet was parsed */
+		st24_rssi = RC_INPUT_RSSI_MAX;
 		*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
 					&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
 	}
-- 
cgit v1.2.3


From f414eef042be7ebf641638009b3676523bf4bed6 Mon Sep 17 00:00:00 2001
From: philipoe <philipp.oettershagen@mavt.ethz.ch>
Date: Tue, 4 Nov 2014 11:10:14 +0100
Subject: TECS: Modify absolute-value limiting of throttle demand

---
 src/lib/external_lgpl/tecs/tecs.cpp | 14 ++++++--------
 1 file changed, 6 insertions(+), 8 deletions(-)

(limited to 'src')

diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 0ed210cf4..cfcc48b62 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -321,31 +321,29 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 			ff_throttle = nomThr - STEdot_dem / _STEdot_min * nomThr;
 		}
 
-		// Calculate PD + FF throttle
+		// Calculate PD + FF throttle and constrain to avoid blow-up of the integrator later
 		_throttle_dem = (_STE_error + STEdot_error * _thrDamp) * K_STE2Thr + ff_throttle;
+		_throttle_dem = constrain(_throttle_dem, _THRminf, _THRmaxf);
 
 		// 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 - thrRateIncr,
+						_last_throttle_dem + thrRateIncr);
 		}
 
 		// Ensure _last_throttle_dem is always initialized properly
-		// Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
 		_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
+		// Set to a value that 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
+		// Set integrator to a max throttle value during climbout
 		_integ6_state = _integ6_state + (_STE_error * _integGain) * _DT * K_STE2Thr;
 
 		if (_climbOutDem) {
-- 
cgit v1.2.3


From 3f3353f2c4ad9f7b8263379c8b31bd9a437b0174 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 5 Nov 2014 21:19:04 +1100
Subject: mixer: fixed stream handle leakage

---
 src/modules/systemlib/mixer/mixer_load.c | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c
index bf3428a50..0d629d610 100644
--- a/src/modules/systemlib/mixer/mixer_load.c
+++ b/src/modules/systemlib/mixer/mixer_load.c
@@ -91,6 +91,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 		/* if the line is too long to fit in the buffer, bail */
 		if ((strlen(line) + strlen(buf) + 1) >= maxlen) {
 			warnx("line too long");
+			fclose(fp);
 			return -1;
 		}
 
@@ -98,6 +99,7 @@ int load_mixer_file(const char *fname, char *buf, unsigned maxlen)
 		strcat(buf, line);
 	}
 
+	fclose(fp);
 	return 0;
 }
 
-- 
cgit v1.2.3


From 43418a674903d242271028c4d4eb473f95a24be6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 7 Nov 2014 13:31:33 +0100
Subject: application layer only, no drivers affected: Fix overflow in RC input
 topic - this topic is deprecated and will be removed, has been superseded by
 input_rc and manual_control

---
 src/modules/uORB/topics/rc_channels.h | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index b14ae1edd..16916cc4d 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -34,6 +34,8 @@
 /**
  * @file rc_channels.h
  * Definition of the rc_channels uORB topic.
+ *
+ * @deprecated DO NOT USE FOR NEW CODE
  */
 
 #ifndef RC_CHANNELS_H_
@@ -63,10 +65,13 @@ enum RC_CHANNELS_FUNCTION {
 	AUX_2,
 	AUX_3,
 	AUX_4,
-	AUX_5,
-	RC_CHANNELS_FUNCTION_MAX /**< Indicates the number of functions. There can be more functions than RC channels. */
+	AUX_5
 };
 
+// MAXIMUM FUNCTIONS IS != MAXIMUM RC INPUT CHANNELS
+
+#define RC_CHANNELS_FUNCTION_MAX 18
+
 /**
  * @addtogroup topics
  * @{
-- 
cgit v1.2.3


From 02d31522cde1de13ab397f4fbd77c0cf9b7f712d Mon Sep 17 00:00:00 2001
From: Daniel Shiels <diyd5@tofubar.com>
Date: Wed, 29 Oct 2014 22:00:30 +1100
Subject: First attempt at sbus1 output.

---
 src/modules/px4iofirmware/sbus.c | 37 ++++++++++++++++++++++++++++++++++---
 1 file changed, 34 insertions(+), 3 deletions(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 6ead38d61..23dae7898 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -80,8 +80,10 @@ static int sbus_fd = -1;
 
 static hrt_abstime last_rx_time;
 static hrt_abstime last_frame_time;
+static hrt_abstime last_txframe_time=0;
 
 static uint8_t	frame[SBUS_FRAME_SIZE];
+static uint8_t	oframe[SBUS_FRAME_SIZE];
 
 static unsigned partial_frame_count;
 
@@ -122,10 +124,39 @@ sbus_init(const char *device)
 void
 sbus1_output(uint16_t *values, uint16_t num_values)
 {
-	char a = 'A';
-	write(sbus_fd, &a, 1);
-}
+	//int. first byte of data is offset 1 in sbus
+	uint8_t byteindex=1;
+	uint8_t offset=0;
+	uint16_t value;
+	hrt_abstime	now;
+	//oframe[0]=0xf0;
+	oframe[0]=0x0f;
 
+        now = hrt_absolute_time();
+        if ((now - last_txframe_time) > 14000) {
+		last_txframe_time = now;
+
+		for (uint16_t i=1;i<SBUS_FRAME_SIZE;++i) {
+			oframe[i]=0;
+		}
+		// 16 is sbus number of servos/channels minus 2 single bit channels.
+		// currently ignoring single bit channels.
+		for (uint16_t i=0;(i<num_values)&&(i<16);++i) {
+			value=(uint16_t)(((values[i]-SBUS_SCALE_OFFSET+.5f)/SBUS_SCALE_FACTOR) +.5f);
+			//protect from out of bounds values and limit to 11 bits;
+			value&=0x07ff;
+			while (offset>=8) {
+				++byteindex;
+				offset-=8;
+			}
+			oframe[byteindex] |= (value<<(offset))&0xff;
+			oframe[byteindex+1]|=(value>>(8-offset))&0xff;
+			oframe[byteindex+2]|=(value>>(16-offset))&0xff;
+			offset+=11;
+		}	
+		write(sbus_fd, oframe, SBUS_FRAME_SIZE);
+	}
+}
 void
 sbus2_output(uint16_t *values, uint16_t num_values)
 {
-- 
cgit v1.2.3


From 60ecd8868db3539e552bad5f51473d1baf5d6ecd Mon Sep 17 00:00:00 2001
From: Daniel Shiels <diyd5@tofubar.com>
Date: Sat, 8 Nov 2014 22:36:45 +1100
Subject: sbus1 output. Cleaned up. Safer bounds checking.

---
 src/modules/px4iofirmware/sbus.c | 45 +++++++++++++++++++++++-----------------
 1 file changed, 26 insertions(+), 19 deletions(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 23dae7898..2f234234e 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -57,6 +57,7 @@
 #define SBUS_FLAGS_BYTE		23
 #define SBUS_FAILSAFE_BIT	3
 #define SBUS_FRAMELOST_BIT	2
+#define SBUS1_FRAME_DELAY	14000
 
 /*
   Measured values with Futaba FX-30/R6108SB:
@@ -80,7 +81,7 @@ static int sbus_fd = -1;
 
 static hrt_abstime last_rx_time;
 static hrt_abstime last_frame_time;
-static hrt_abstime last_txframe_time=0;
+static hrt_abstime last_txframe_time = 0;
 
 static uint8_t	frame[SBUS_FRAME_SIZE];
 static uint8_t	oframe[SBUS_FRAME_SIZE];
@@ -125,35 +126,41 @@ void
 sbus1_output(uint16_t *values, uint16_t num_values)
 {
 	//int. first byte of data is offset 1 in sbus
-	uint8_t byteindex=1;
-	uint8_t offset=0;
+	uint8_t byteindex = 1;
+	uint8_t offset = 0;
 	uint16_t value;
 	hrt_abstime	now;
-	//oframe[0]=0xf0;
-	oframe[0]=0x0f;
+	oframe[0] = 0x0f;
 
-        now = hrt_absolute_time();
-        if ((now - last_txframe_time) > 14000) {
+	now = hrt_absolute_time();
+
+	if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
 		last_txframe_time = now;
 
-		for (uint16_t i=1;i<SBUS_FRAME_SIZE;++i) {
-			oframe[i]=0;
+		for (uint16_t i = 1; i < SBUS_FRAME_SIZE; ++i) {
+			oframe[i] = 0;
 		}
+
 		// 16 is sbus number of servos/channels minus 2 single bit channels.
 		// currently ignoring single bit channels.
-		for (uint16_t i=0;(i<num_values)&&(i<16);++i) {
-			value=(uint16_t)(((values[i]-SBUS_SCALE_OFFSET+.5f)/SBUS_SCALE_FACTOR) +.5f);
+		for (uint16_t i = 0; (i < num_values) && (i < 16); ++i) {
+			value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f);
 			//protect from out of bounds values and limit to 11 bits;
-			value&=0x07ff;
-			while (offset>=8) {
+			if (value > 0x07ff ) {
+				value = 0x07ff;
+			}
+
+			while (offset >= 8) {
 				++byteindex;
-				offset-=8;
+				offset -= 8;
 			}
-			oframe[byteindex] |= (value<<(offset))&0xff;
-			oframe[byteindex+1]|=(value>>(8-offset))&0xff;
-			oframe[byteindex+2]|=(value>>(16-offset))&0xff;
-			offset+=11;
-		}	
+
+			oframe[byteindex] |= (value << (offset)) & 0xff;
+			oframe[byteindex + 1] |= (value >> (8 - offset)) & 0xff;
+			oframe[byteindex + 2] |= (value >> (16 - offset)) & 0xff;
+			offset += 11;
+		}
+
 		write(sbus_fd, oframe, SBUS_FRAME_SIZE);
 	}
 }
-- 
cgit v1.2.3


From 62db84fa758ecc30081e30a18464fe3060d2c2d8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 9 Nov 2014 18:20:53 +0100
Subject: MAVLink update compile fixes

---
 src/modules/mavlink/mavlink.c            |  6 +-----
 src/modules/mavlink/mavlink_main.cpp     | 12 +++++++-----
 src/modules/mavlink/mavlink_main.h       |  4 ++++
 src/modules/mavlink/mavlink_messages.cpp | 12 +++++++-----
 4 files changed, 19 insertions(+), 15 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index bec706bad..9afe74af1 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -78,11 +78,7 @@ PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
 
 mavlink_system_t mavlink_system = {
 	100,
-	50,
-	MAV_TYPE_FIXED_WING,
-	0,
-	0,
-	0
+	50
 }; // System ID, 1-255, Component/Subsystem ID, 1-255
 
 /*
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 6b4edff78..c251dd3b2 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -167,8 +167,10 @@ Mavlink::Mavlink() :
 	_param_initialized(false),
 	_param_system_id(0),
 	_param_component_id(0),
-	_param_system_type(0),
+	_param_system_type(MAV_TYPE_FIXED_WING),
 	_param_use_hil_gps(0),
+	_param_forward_externalsp(0),
+	_system_type(0),
 
 	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
@@ -524,7 +526,7 @@ void Mavlink::mavlink_update_system(void)
 	param_get(_param_system_type, &system_type);
 
 	if (system_type >= 0 && system_type < MAV_TYPE_ENUM_END) {
-		mavlink_system.type = system_type;
+		_system_type = system_type;
 	}
 
 	int32_t use_hil_gps;
@@ -755,7 +757,7 @@ Mavlink::send_message(const uint8_t msgid, const void *msg)
 
 	pthread_mutex_lock(&_send_mutex);
 
-	int buf_free = get_free_tx_buf();
+	unsigned buf_free = get_free_tx_buf();
 
 	uint8_t payload_len = mavlink_message_lengths[msgid];
 	unsigned packet_len = payload_len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
@@ -820,14 +822,14 @@ Mavlink::resend_message(mavlink_message_t *msg)
 
 	pthread_mutex_lock(&_send_mutex);
 
-	int buf_free = get_free_tx_buf();
+	unsigned buf_free = get_free_tx_buf();
 
 	_last_write_try_time = hrt_absolute_time();
 
 	unsigned packet_len = msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 
 	/* check if there is space in the buffer, let it overflow else */
-	if (buf_free < TX_BUFFER_GAP) {
+	if ((buf_free < TX_BUFFER_GAP) || (buf_free < packet_len)) {
 		/* no enough space in buffer to send */
 		count_txerr();
 		count_txerrbytes(packet_len);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1f96e586b..ad5e5001b 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -265,6 +265,8 @@ public:
 
 	struct mavlink_logbuffer	*get_logbuffer() { return &_logbuffer; }
 
+	unsigned		get_system_type() { return _system_type; }
+
 protected:
 	Mavlink			*next;
 
@@ -354,6 +356,8 @@ private:
 	param_t			_param_use_hil_gps;
 	param_t			_param_forward_externalsp;
 
+	unsigned		_system_type;
+
 	perf_counter_t		_loop_perf;			/**< loop performance counter */
 	perf_counter_t		_txerr_perf;			/**< TX error counter */
 
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index cccb698bf..b4911427f 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -302,7 +302,7 @@ protected:
 		msg.base_mode = 0;
 		msg.custom_mode = 0;
 		get_mavlink_mode_state(&status, &pos_sp_triplet, &msg.system_status, &msg.base_mode, &msg.custom_mode);
-		msg.type = mavlink_system.type;
+		msg.type = _mavlink->get_system_type();
 		msg.autopilot = MAV_AUTOPILOT_PX4;
 		msg.mavlink_version = 3;
 
@@ -1353,15 +1353,17 @@ protected:
 
 			const float pwm_center = (PWM_HIGHEST_MAX + PWM_LOWEST_MIN) / 2;
 
+			unsigned system_type = _mavlink->get_system_type();
+
 			/* scale outputs depending on system type */
-			if (mavlink_system.type == MAV_TYPE_QUADROTOR ||
-				mavlink_system.type == MAV_TYPE_HEXAROTOR ||
-				mavlink_system.type == MAV_TYPE_OCTOROTOR) {
+			if (system_type == MAV_TYPE_QUADROTOR ||
+				system_type == MAV_TYPE_HEXAROTOR ||
+				system_type == MAV_TYPE_OCTOROTOR) {
 				/* multirotors: set number of rotor outputs depending on type */
 
 				unsigned n;
 
-				switch (mavlink_system.type) {
+				switch (system_type) {
 				case MAV_TYPE_QUADROTOR:
 					n = 4;
 					break;
-- 
cgit v1.2.3


From 34e9d9efce10ce487b4c5af0cf93a442ab3b8594 Mon Sep 17 00:00:00 2001
From: Daniel Shiels <diyd5@tofubar.com>
Date: Mon, 10 Nov 2014 15:54:28 +1100
Subject: Code style cleanup.

---
 src/modules/px4iofirmware/sbus.c | 13 +++++++------
 1 file changed, 7 insertions(+), 6 deletions(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 2f234234e..90c0e0503 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -125,8 +125,7 @@ sbus_init(const char *device)
 void
 sbus1_output(uint16_t *values, uint16_t num_values)
 {
-	//int. first byte of data is offset 1 in sbus
-	uint8_t byteindex = 1;
+	uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
 	uint8_t offset = 0;
 	uint16_t value;
 	hrt_abstime	now;
@@ -141,11 +140,13 @@ sbus1_output(uint16_t *values, uint16_t num_values)
 			oframe[i] = 0;
 		}
 
-		// 16 is sbus number of servos/channels minus 2 single bit channels.
-		// currently ignoring single bit channels.
-		for (uint16_t i = 0; (i < num_values) && (i < 16); ++i) {
+		/* 16 is sbus number of servos/channels minus 2 single bit channels.
+		* currently ignoring single bit channels.  */
+
+		for (unsigned i = 0; (i < num_values) && (i < 16); ++i) {
 			value = (uint16_t)(((values[i] - SBUS_SCALE_OFFSET) / SBUS_SCALE_FACTOR) + .5f);
-			//protect from out of bounds values and limit to 11 bits;
+
+			/*protect from out of bounds values and limit to 11 bits*/
 			if (value > 0x07ff ) {
 				value = 0x07ff;
 			}
-- 
cgit v1.2.3


From 3eeec2cce05c8f32a5c2d70b956786d41114df3c Mon Sep 17 00:00:00 2001
From: Daniel Shiels <diyd5@tofubar.com>
Date: Mon, 10 Nov 2014 16:34:28 +1100
Subject: Cleaned up sbus output frame initialisation.

---
 src/modules/px4iofirmware/sbus.c | 9 ++-------
 1 file changed, 2 insertions(+), 7 deletions(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c
index 90c0e0503..d76ec55f0 100644
--- a/src/modules/px4iofirmware/sbus.c
+++ b/src/modules/px4iofirmware/sbus.c
@@ -84,7 +84,6 @@ static hrt_abstime last_frame_time;
 static hrt_abstime last_txframe_time = 0;
 
 static uint8_t	frame[SBUS_FRAME_SIZE];
-static uint8_t	oframe[SBUS_FRAME_SIZE];
 
 static unsigned partial_frame_count;
 
@@ -128,17 +127,13 @@ sbus1_output(uint16_t *values, uint16_t num_values)
 	uint8_t byteindex = 1; /*Data starts one byte into the sbus frame. */
 	uint8_t offset = 0;
 	uint16_t value;
-	hrt_abstime	now;
-	oframe[0] = 0x0f;
+	hrt_abstime now;
 
 	now = hrt_absolute_time();
 
 	if ((now - last_txframe_time) > SBUS1_FRAME_DELAY) {
 		last_txframe_time = now;
-
-		for (uint16_t i = 1; i < SBUS_FRAME_SIZE; ++i) {
-			oframe[i] = 0;
-		}
+		uint8_t	oframe[SBUS_FRAME_SIZE] = { 0x0f };
 
 		/* 16 is sbus number of servos/channels minus 2 single bit channels.
 		* currently ignoring single bit channels.  */
-- 
cgit v1.2.3


From ea0a59f80650c7bd7c72889f1c7bb72bfd3fdbd5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 10 Nov 2014 09:26:11 +0100
Subject: HMC5883: Reduce output, add indices to make back-tracking lines
 easier, remove output completely for an OK-operation (the driver is NOT the
 right place to talk to the user!)

---
 src/drivers/hmc5883/hmc5883.cpp | 49 +++++++++++++----------------------------
 1 file changed, 15 insertions(+), 34 deletions(-)

(limited to 'src')

diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp
index e4ecfa6b5..81f767965 100644
--- a/src/drivers/hmc5883/hmc5883.cpp
+++ b/src/drivers/hmc5883/hmc5883.cpp
@@ -1049,11 +1049,9 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	 * LSM/Ga, giving 1.16 and 1.08 */
 	float expected_cal[3] = { 1.16f, 1.08f, 1.08f };
 
-	warnx("starting mag scale calibration");
-
 	/* start the sensor polling at 50 Hz */
 	if (OK != ioctl(filp, SENSORIOCSPOLLRATE, 50)) {
-		warn("failed to set 2Hz poll rate");
+		warn("FAILED: SENSORIOCSPOLLRATE 2Hz");
 		ret = 1;
 		goto out;
 	}
@@ -1061,25 +1059,25 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 	/* Set to 2.5 Gauss. We ask for 3 to get the right part of
          * the chained if statement above. */
 	if (OK != ioctl(filp, MAGIOCSRANGE, 3)) {
-		warnx("failed to set 2.5 Ga range");
+		warnx("FAILED: MAGIOCSRANGE 3.3 Ga");
 		ret = 1;
 		goto out;
 	}
 
 	if (OK != ioctl(filp, MAGIOCEXSTRAP, 1)) {
-		warnx("failed to enable sensor calibration mode");
+		warnx("FAILED: MAGIOCEXSTRAP 1");
 		ret = 1;
 		goto out;
 	}
 
 	if (OK != ioctl(filp, MAGIOCGSCALE, (long unsigned int)&mscale_previous)) {
-		warn("WARNING: failed to get scale / offsets for mag");
+		warn("FAILED: MAGIOCGSCALE 1");
 		ret = 1;
 		goto out;
 	}
 
 	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
-		warn("WARNING: failed to set null scale / offsets for mag");
+		warn("FAILED: MAGIOCSSCALE 1");
 		ret = 1;
 		goto out;
 	}
@@ -1094,7 +1092,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		ret = ::poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			warn("timed out waiting for sensor data");
+			warn("ERROR: TIMEOUT 1");
 			goto out;
 		}
 
@@ -1102,7 +1100,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		sz = ::read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			warn("periodic read failed");
+			warn("ERROR: READ 1");
 			ret = -EIO;
 			goto out;
 		}
@@ -1118,7 +1116,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		ret = ::poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			warn("timed out waiting for sensor data");
+			warn("ERROR: TIMEOUT 2");
 			goto out;
 		}
 
@@ -1126,7 +1124,7 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 		sz = ::read(fd, &report, sizeof(report));
 
 		if (sz != sizeof(report)) {
-			warn("periodic read failed");
+			warn("ERROR: READ 2");
 			ret = -EIO;
 			goto out;
 		}
@@ -1142,33 +1140,19 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 			sum_excited[1] += cal[1];
 			sum_excited[2] += cal[2];
 		}
-
-		//warnx("periodic read %u", i);
-		//warnx("measurement: %.6f  %.6f  %.6f", (double)report.x, (double)report.y, (double)report.z);
-		//warnx("cal: %.6f  %.6f  %.6f", (double)cal[0], (double)cal[1], (double)cal[2]);
 	}
 
 	if (good_count < 5) {
-		warn("failed calibration");
 		ret = -EIO;
 		goto out;
 	}
 
-#if 0
-	warnx("measurement avg: %.6f  %.6f  %.6f", 
-	      (double)sum_excited[0]/good_count, 
-	      (double)sum_excited[1]/good_count, 
-	      (double)sum_excited[2]/good_count);
-#endif
-
 	float scaling[3];
 
 	scaling[0] = sum_excited[0] / good_count;
 	scaling[1] = sum_excited[1] / good_count;
 	scaling[2] = sum_excited[2] / good_count;
 
-	warnx("axes scaling: %.6f  %.6f  %.6f", (double)scaling[0], (double)scaling[1], (double)scaling[2]);
-
 	/* set scaling in device */
 	mscale_previous.x_scale = scaling[0];
 	mscale_previous.y_scale = scaling[1];
@@ -1179,29 +1163,26 @@ int HMC5883::calibrate(struct file *filp, unsigned enable)
 out:
 
 	if (OK != ioctl(filp, MAGIOCSSCALE, (long unsigned int)&mscale_previous)) {
-		warn("failed to set new scale / offsets for mag");
+		warn("FAILED: MAGIOCSSCALE 2");
 	}
 
 	/* set back to normal mode */
 	/* Set to 1.1 Gauss */
 	if (OK != ::ioctl(fd, MAGIOCSRANGE, 1)) {
-		warnx("failed to set 1.1 Ga range");
+		warnx("FAILED: MAGIOCSRANGE 1.1 Ga");
 	}
 
 	if (OK != ::ioctl(fd, MAGIOCEXSTRAP, 0)) {
-		warnx("failed to disable sensor calibration mode");
+		warnx("FAILED: MAGIOCEXSTRAP 0");
 	}
 
 	if (ret == OK) {
-		if (!check_scale()) {
-			warnx("mag scale calibration successfully finished.");
-		} else {
-			warnx("mag scale calibration finished with invalid results.");
+		if (check_scale()) {
+			/* failed */
+			warnx("FAILED: SCALE");
 			ret = ERROR;
 		}
 
-	} else {
-		warnx("mag scale calibration failed.");
 	}
 
 	return ret;
-- 
cgit v1.2.3


From d253c8ba7d9cfc408328afd7c3708892b23626c0 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 10 Nov 2014 21:38:13 +1000
Subject: commander: correct the description array

---
 src/modules/commander/commander.cpp | 3 +++
 1 file changed, 3 insertions(+)

(limited to 'src')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 46caddd46..1c3b68edd 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -760,7 +760,10 @@ int commander_thread_main(int argc, char *argv[])
 	nav_states_str[NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
 	nav_states_str[NAVIGATION_STATE_AUTO_LOITER]		= "AUTO_LOITER";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTL]		= "AUTO_RTL";
+	nav_states_str[NAVIGATION_STATE_AUTO_RCRECOVER]		= "AUTO_RCRECOVER";
 	nav_states_str[NAVIGATION_STATE_AUTO_RTGS]		= "AUTO_RTGS";
+	nav_states_str[NAVIGATION_STATE_AUTO_LANDENGFAIL]	= "AUTO_LANDENGFAIL";
+	nav_states_str[NAVIGATION_STATE_AUTO_LANDGPSFAIL]	= "AUTO_LANDGPSFAIL";
 	nav_states_str[NAVIGATION_STATE_ACRO]			= "ACRO";
 	nav_states_str[NAVIGATION_STATE_LAND]			= "LAND";
 	nav_states_str[NAVIGATION_STATE_DESCEND]		= "DESCEND";
-- 
cgit v1.2.3


From f7db91ac32d6db1fbef9aa0c868636561bd2282c Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 10 Nov 2014 21:38:46 +1000
Subject: commander: enhance the failsafe verbose output

---
 src/modules/commander/commander.cpp | 6 +++++-
 1 file changed, 5 insertions(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 1c3b68edd..f9ea4e914 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1851,7 +1851,11 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (status.failsafe != failsafe_old) {
 			status_changed = true;
-			mavlink_log_info(mavlink_fd, "[cmd] failsafe state: %i", status.failsafe);
+			if (status.failsafe) {
+				mavlink_log_info(mavlink_fd, "[cmd] failsafe on");
+			} else {
+				mavlink_log_info(mavlink_fd, "[cmd] failsafe off");
+			}
 			failsafe_old = status.failsafe;
 		}
 
-- 
cgit v1.2.3


From f8bed3cd892157b2db147b0d6a60f26371acdeb8 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 10 Nov 2014 21:39:35 +1000
Subject: state_machine_helper: whitespace

---
 src/modules/commander/state_machine_helper.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 9568752ae..a49965efd 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -444,7 +444,7 @@ transition_result_t hil_state_transition(hil_state_t new_state, int status_pub,
  * Check failsafe and main status and set navigation status for navigator accordingly
  */
 bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_enabled, const bool mission_finished,
-		const bool stay_in_failsafe)
+		   const bool stay_in_failsafe)
 {
 	navigation_state_t nav_state_old = status->nav_state;
 
-- 
cgit v1.2.3


From 2c577815818b579743dab13ef0055f02605699d1 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 10 Nov 2014 21:40:13 +1000
Subject: state_machine_helper: trying to clean up some failsafe logic

---
 src/modules/commander/state_machine_helper.cpp | 43 ++++++++++++++++++--------
 1 file changed, 30 insertions(+), 13 deletions(-)

(limited to 'src')

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index a49965efd..3553dcdc3 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -497,11 +497,14 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		break;
 
 	case MAIN_STATE_AUTO_MISSION:
+
 		/* go into failsafe
 		 * - if commanded to do so
 		 * - if we have an engine failure
-		 * - if either the datalink is enabled and lost as well as RC is lost
-		 * - if there is no datalink and the mission is finished */
+		 * - if the datalink is enabled and lost as well as RC is lost
+		 *   or if the datalink is disabled and lost as well as RC is lost and the mission is finished */
+
+		/* first look at the commands */
 		if (status->engine_failure_cmd) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
 		} else if (status->data_link_lost_cmd) {
@@ -509,14 +512,16 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		} else if (status->gps_failure_cmd) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		} else if (status->rc_signal_lost_cmd) {
-			status->nav_state = NAVIGATION_STATE_AUTO_RTGS; //XXX
-		/* Finished handling commands which have priority , now handle failures */
+			status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+
+		/* finished handling commands which have priority, now handle failures */
 		} else if (status->gps_failure) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDGPSFAIL;
 		} else if (status->engine_failure) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
-		} else if (((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) ||
-		    (!data_link_loss_enabled && status->rc_signal_lost && mission_finished)) {
+
+		/* check for RC signal and datalink lost (with datalink enabled) after mission is finished */
+		} else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -529,7 +534,21 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* also go into failsafe if just datalink is lost */
+		/* check if RC signal is lost (with dataink disabled) after mission is finished*/
+		} else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) {
+			status->failsafe = true;
+
+			if (status->condition_global_position_valid && status->condition_home_position_valid) {
+				status->nav_state = NAVIGATION_STATE_AUTO_RCRECOVER;
+			} else if (status->condition_local_position_valid) {
+				status->nav_state = NAVIGATION_STATE_LAND;
+			} else if (status->condition_local_altitude_valid) {
+				status->nav_state = NAVIGATION_STATE_DESCEND;
+			} else {
+				status->nav_state = NAVIGATION_STATE_TERMINATION;
+			}
+
+		/* also go into failsafe if just datalink is lost (with datalink enabled) */
 		} else if (status->data_link_lost && data_link_loss_enabled) {
 			status->failsafe = true;
 
@@ -543,13 +562,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* don't bother if RC is lost and mission is not yet finished */
-		} else if (status->rc_signal_lost && !stay_in_failsafe) {
+		/* stay where you are */
+		} else if (stay_in_failsafe){
 
-			/* this mode is ok, we don't need RC for missions */
-			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
-		} else if (!stay_in_failsafe){
-			/* everything is perfect */
+		/* everything is perfect */
+		} else {
 			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
 		}
 		break;
-- 
cgit v1.2.3


From f1d56cbddc575115215f680ba42cdedde40b2626 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 10 Nov 2014 21:41:41 +1000
Subject: navigator: don't reset the finished flag, this fixes the strange
 problem where it toggles between MISSION and RTL

---
 src/modules/navigator/navigator_main.cpp | 1 -
 1 file changed, 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index a867dd0da..eff3a5938 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -632,7 +632,6 @@ Navigator::publish_mission_result()
 	}
 	/* reset reached bool */
 	_mission_result.reached = false;
-	_mission_result.finished = false;
 }
 
 void
-- 
cgit v1.2.3


From 89724c24a16bd340c33c27531435089e7bd338fb Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Mon, 10 Nov 2014 21:42:13 +1000
Subject: vehicle_status: whitespace

---
 src/modules/uORB/topics/vehicle_status.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index a1b2667e3..91491c148 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -108,7 +108,7 @@ typedef enum {
 	NAVIGATION_STATE_AUTO_LANDGPSFAIL,	/**< Auto land on gps failure (e.g. open loop loiter down) */
 	NAVIGATION_STATE_ACRO,			/**< Acro mode */
 	NAVIGATION_STATE_LAND,			/**< Land mode */
-	NAVIGATION_STATE_DESCEND,			/**< Descend mode (no position control) */
+	NAVIGATION_STATE_DESCEND,		/**< Descend mode (no position control) */
 	NAVIGATION_STATE_TERMINATION,		/**< Termination mode */
 	NAVIGATION_STATE_OFFBOARD,
 	NAVIGATION_STATE_MAX,
-- 
cgit v1.2.3


From 88093ebf1af5483d8b5c218adb1ea0c4798ee21e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 10 Nov 2014 18:52:18 +0100
Subject: Hotfix: Fix IO channel mapping

---
 src/drivers/px4io/px4io.cpp | 47 +++++++++++++++++++++++----------------------
 1 file changed, 24 insertions(+), 23 deletions(-)

(limited to 'src')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 6d68d9f60..06faf49a4 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1245,41 +1245,42 @@ PX4IO::io_set_rc_config()
 	 *       for compatibility reasons with existing
 	 *       autopilots / GCS'.
 	 */
-	param_get(param_find("RC_MAP_ROLL"), &ichan);
-
-	/* subtract one from 1-based index - this might be
-	 * a negative number now
-	 */
-	ichan -= 1;
 
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan] = 0;
+	/* ROLL */
+	param_get(param_find("RC_MAP_ROLL"), &ichan);
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+		input_map[ichan - 1] = 0;
+	}
 
+	/* PITCH */
 	param_get(param_find("RC_MAP_PITCH"), &ichan);
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+		input_map[ichan - 1] = 1;
+	}
 
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan] = 1;
-
+	/* YAW */
 	param_get(param_find("RC_MAP_YAW"), &ichan);
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+		input_map[ichan - 1] = 2;
+	}
 
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan] = 2;
-
+	/* THROTTLE */
 	param_get(param_find("RC_MAP_THROTTLE"), &ichan);
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+		input_map[ichan - 1] = 3;
+	}
 
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan] = 3;
-
+	/* FLAPS */
 	param_get(param_find("RC_MAP_FLAPS"), &ichan);
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
+		input_map[ichan - 1] = 4;
+	}
 
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input))
-		input_map[ichan] = 4;
-
+	/* MAIN MODE SWITCH */
 	param_get(param_find("RC_MAP_MODE_SW"), &ichan);
-
-	if ((ichan >= 0) && (ichan < (int)_max_rc_input)) {
+	if ((ichan > 0) && (ichan <= (int)_max_rc_input)) {
 		/* use out of normal bounds index to indicate special channel */
-		input_map[ichan] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
+		input_map[ichan - 1] = PX4IO_P_RC_CONFIG_ASSIGNMENT_MODESWITCH;
 	}
 
 	/*
-- 
cgit v1.2.3


From e474600978f0fe92de79622a746d06f2c9df77a9 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 11 Nov 2014 09:18:31 +1000
Subject: commander: make the failsafe message critical

---
 src/modules/commander/commander.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index f9ea4e914..002a33811 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1852,9 +1852,9 @@ int commander_thread_main(int argc, char *argv[])
 		if (status.failsafe != failsafe_old) {
 			status_changed = true;
 			if (status.failsafe) {
-				mavlink_log_info(mavlink_fd, "[cmd] failsafe on");
+				mavlink_log_critical(mavlink_fd, "failsafe mode on");
 			} else {
-				mavlink_log_info(mavlink_fd, "[cmd] failsafe off");
+				mavlink_log_critical(mavlink_fd, "failsafe mode off");
 			}
 			failsafe_old = status.failsafe;
 		}
-- 
cgit v1.2.3


From fa5f3658828280f54a7fec3c33ec3e25e4198561 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 11 Nov 2014 10:25:53 +1000
Subject: state_machine_helper: fixed comments

---
 src/modules/commander/state_machine_helper.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 3553dcdc3..e1a626262 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -520,7 +520,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		} else if (status->engine_failure) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
 
-		/* check for RC signal and datalink lost (with datalink enabled) after mission is finished */
+		/* check for RC signal and datalink lost (with datalink enabled) */
 		} else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
 			status->failsafe = true;
 
@@ -534,7 +534,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* check if RC signal is lost (with dataink disabled) after mission is finished*/
+		/* check if RC signal is lost (with datalink disabled) after mission is finished */
 		} else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) {
 			status->failsafe = true;
 
-- 
cgit v1.2.3


From 3c9a73f3e41a379e5e7bbf6e98d67facc3b2b49f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 11 Nov 2014 10:28:24 +1000
Subject: fixed empty if body

---
 src/modules/commander/state_machine_helper.cpp | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

(limited to 'src')

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index e1a626262..a9a5c4051 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -562,11 +562,8 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* stay where you are */
-		} else if (stay_in_failsafe){
-
-		/* everything is perfect */
-		} else {
+		/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
+		} else if (!stay_in_failsafe){
 			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
 		}
 		break;
-- 
cgit v1.2.3


From d98831c3b50addab0084f9636060cece46ff92bc Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 11 Nov 2014 10:42:26 +1000
Subject: navigator: move waypoint reached reset to a more obvious location

---
 src/modules/navigator/mission.cpp        | 1 +
 src/modules/navigator/navigator_main.cpp | 2 --
 2 files changed, 1 insertion(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 7fac69a61..b8e3274fd 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -707,6 +707,7 @@ Mission::set_mission_item_reached()
 	_navigator->get_mission_result()->reached = true;
 	_navigator->get_mission_result()->seq_reached = _current_offboard_mission_index;
 	_navigator->publish_mission_result();
+	reset_mission_item_reached();
 }
 
 void
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index eff3a5938..e933919fb 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -630,8 +630,6 @@ Navigator::publish_mission_result()
 		/* advertise and publish */
 		_mission_result_pub = orb_advertise(ORB_ID(mission_result), &_mission_result);
 	}
-	/* reset reached bool */
-	_mission_result.reached = false;
 }
 
 void
-- 
cgit v1.2.3


From 51ffb887c39ecfd0cb94879475f032ee65505321 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Tue, 11 Nov 2014 11:13:08 +0100
Subject: UAVCAN: initialize device id for mag and baro to allow
 DEVIOCGDEVICEID ioctl to return useful data

---
 src/drivers/device/device.h                  | 3 ++-
 src/modules/uavcan/sensors/mag.cpp           | 2 ++
 src/modules/uavcan/sensors/sensor_bridge.cpp | 3 +++
 src/modules/uavcan/sensors/sensor_bridge.hpp | 2 ++
 4 files changed, 9 insertions(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/drivers/device/device.h b/src/drivers/device/device.h
index 9d684e394..67aaa0aff 100644
--- a/src/drivers/device/device.h
+++ b/src/drivers/device/device.h
@@ -130,7 +130,8 @@ public:
 	enum DeviceBusType {
 		DeviceBusType_UNKNOWN = 0,
 		DeviceBusType_I2C     = 1,
-		DeviceBusType_SPI     = 2
+		DeviceBusType_SPI     = 2,
+		DeviceBusType_UAVCAN  = 3,
 	};
 
 	/*
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 8e6a9a22f..0d9ea08c5 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -49,6 +49,8 @@ UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) :
 UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS),
 _sub_mag(node)
 {
+	_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883;
+
 	_scale.x_scale = 1.0F;
 	_scale.y_scale = 1.0F;
 	_scale.z_scale = 1.0F;
diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp
index 9608ce680..0999938fc 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.cpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.cpp
@@ -103,6 +103,9 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report)
 			return;
 		}
 
+		// update device id as we now know our device node_id
+		_device_id.devid_s.address = static_cast<uint8_t>(node_id);
+
 		// Ask the CDev helper which class instance we can take
 		const int class_instance = register_class_devname(_class_devname);
 		if (class_instance < 0 || class_instance >= int(_max_channels)) {
diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
index 1316f7ecc..04c59b77d 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.hpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -112,6 +112,8 @@ protected:
 	_channels(new Channel[MaxChannels])
 	{
 		memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
+		_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
+		_device_id.devid_s.bus = 1; // @TBD: insert UAVCAN bus no. here
 	}
 
 	/**
-- 
cgit v1.2.3


From cf2baac516b6f86daf30342be47264b2dcc8abc4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Tue, 11 Nov 2014 21:45:13 +1000
Subject: navigator: more reset reached calls

---
 src/modules/navigator/datalinkloss.cpp | 4 ++++
 src/modules/navigator/rcloss.cpp       | 2 ++
 2 files changed, 6 insertions(+)

(limited to 'src')

diff --git a/src/modules/navigator/datalinkloss.cpp b/src/modules/navigator/datalinkloss.cpp
index 66f1c8c73..e789fd10d 100644
--- a/src/modules/navigator/datalinkloss.cpp
+++ b/src/modules/navigator/datalinkloss.cpp
@@ -156,6 +156,7 @@ DataLinkLoss::set_dll_item()
 		/* Request flight termination from the commander */
 		_navigator->get_mission_result()->flight_termination = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		warnx("not switched to manual: request flight termination");
 		pos_sp_triplet->previous.valid = false;
 		pos_sp_triplet->current.valid = false;
@@ -188,6 +189,7 @@ DataLinkLoss::advance_dll()
 			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
 			_navigator->get_mission_result()->stay_in_failsafe = true;
 			_navigator->publish_mission_result();
+			reset_mission_item_reached();
 			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		} else {
 			if (!_param_skipcommshold.get()) {
@@ -208,6 +210,7 @@ DataLinkLoss::advance_dll()
 		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		break;
 	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
 		_dll_state = DLL_STATE_TERMINATE;
@@ -215,6 +218,7 @@ DataLinkLoss::advance_dll()
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		break;
 	case DLL_STATE_TERMINATE:
 		warnx("dll end");
diff --git a/src/modules/navigator/rcloss.cpp b/src/modules/navigator/rcloss.cpp
index 5564a1c42..42392e739 100644
--- a/src/modules/navigator/rcloss.cpp
+++ b/src/modules/navigator/rcloss.cpp
@@ -163,6 +163,7 @@ RCLoss::advance_rcl()
 			_rcl_state = RCL_STATE_TERMINATE;
 			_navigator->get_mission_result()->stay_in_failsafe = true;
 			_navigator->publish_mission_result();
+			reset_mission_item_reached();
 		}
 		break;
 	case RCL_STATE_LOITER:
@@ -171,6 +172,7 @@ RCLoss::advance_rcl()
 		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
 		_navigator->get_mission_result()->stay_in_failsafe = true;
 		_navigator->publish_mission_result();
+		reset_mission_item_reached();
 		break;
 	case RCL_STATE_TERMINATE:
 		warnx("rcl end");
-- 
cgit v1.2.3


From 50b410664f4087bb4fd50e8bba62267c70719b71 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Tue, 11 Nov 2014 17:01:20 +0100
Subject: UAVCAN: set bus number part of device_id to zero

---
 src/modules/uavcan/sensors/sensor_bridge.hpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp
index 04c59b77d..e31960537 100644
--- a/src/modules/uavcan/sensors/sensor_bridge.hpp
+++ b/src/modules/uavcan/sensors/sensor_bridge.hpp
@@ -113,7 +113,7 @@ protected:
 	{
 		memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels);
 		_device_id.devid_s.bus_type = DeviceBusType_UAVCAN;
-		_device_id.devid_s.bus = 1; // @TBD: insert UAVCAN bus no. here
+		_device_id.devid_s.bus = 0;
 	}
 
 	/**
-- 
cgit v1.2.3


From d63c054bb00a0df6666f498950ab797dc07817b4 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 12 Nov 2014 09:48:32 +1000
Subject: state_machine_helper: changed failsafe behaviour: always require at
 least one link for default, do RTGS as soon as datalink is lost if datalink
 loss mode is enabled

---
 src/modules/commander/state_machine_helper.cpp | 29 ++++++++------------------
 1 file changed, 9 insertions(+), 20 deletions(-)

(limited to 'src')

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index a9a5c4051..e37019d02 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -501,8 +501,7 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		/* go into failsafe
 		 * - if commanded to do so
 		 * - if we have an engine failure
-		 * - if the datalink is enabled and lost as well as RC is lost
-		 *   or if the datalink is disabled and lost as well as RC is lost and the mission is finished */
+		 * - depending on datalink, RC and if the mission is finished */
 
 		/* first look at the commands */
 		if (status->engine_failure_cmd) {
@@ -520,8 +519,9 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 		} else if (status->engine_failure) {
 			status->nav_state = NAVIGATION_STATE_AUTO_LANDENGFAIL;
 
-		/* check for RC signal and datalink lost (with datalink enabled) */
-		} else if ((status->data_link_lost && data_link_loss_enabled) && status->rc_signal_lost) {
+		/* datalink loss enabled:
+		 * check for datalink lost: this should always trigger RTGS */
+		} else if (data_link_loss_enabled && status->data_link_lost) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -534,8 +534,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* check if RC signal is lost (with datalink disabled) after mission is finished */
-		} else if (!data_link_loss_enabled && status->rc_signal_lost && mission_finished) {
+		/* datalink loss disabled:
+		 * check if both, RC and datalink are lost during the mission
+		 * or RC is lost after the mission is finished: this should always trigger RCRECOVER */
+		} else if (!data_link_loss_enabled && ((status->rc_signal_lost && status->data_link_lost) ||
+						       (status->rc_signal_lost && mission_finished))) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -548,20 +551,6 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = NAVIGATION_STATE_TERMINATION;
 			}
 
-		/* also go into failsafe if just datalink is lost (with datalink enabled) */
-		} else if (status->data_link_lost && data_link_loss_enabled) {
-			status->failsafe = true;
-
-			if (status->condition_global_position_valid && status->condition_home_position_valid) {
-				status->nav_state = NAVIGATION_STATE_AUTO_RTGS;
-			} else if (status->condition_local_position_valid) {
-				status->nav_state = NAVIGATION_STATE_LAND;
-			} else if (status->condition_local_altitude_valid) {
-				status->nav_state = NAVIGATION_STATE_DESCEND;
-			} else {
-				status->nav_state = NAVIGATION_STATE_TERMINATION;
-			}
-
 		/* stay where you are if you should stay in failsafe, otherwise everything is perfect */
 		} else if (!stay_in_failsafe){
 			status->nav_state = NAVIGATION_STATE_AUTO_MISSION;
-- 
cgit v1.2.3


From 20703c8d2352dd137ec13be69e42fe6fdfd9f8f2 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 12 Nov 2014 11:05:06 +1000
Subject: manual_control_setpoint: whitespace

---
 src/modules/uORB/topics/manual_control_setpoint.h | 14 +++++++-------
 1 file changed, 7 insertions(+), 7 deletions(-)

(limited to 'src')

diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index dde237adc..c0c33a629 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -47,7 +47,7 @@
  * Switch position
  */
 typedef enum {
-	SWITCH_POS_NONE = 0,	/**< switch is not mapped */
+	SWITCH_POS_NONE = 0,		/**< switch is not mapped */
 	SWITCH_POS_ON,			/**< switch activated (value = 1) */
 	SWITCH_POS_MIDDLE,		/**< middle position (value = 0) */
 	SWITCH_POS_OFF			/**< switch not activated (value = -1) */
@@ -93,12 +93,12 @@ struct manual_control_setpoint_s {
 	float aux4;			/**< default function: camera roll */
 	float aux5;			/**< default function: payload drop */
 
-	switch_pos_t mode_switch;			/**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
-	switch_pos_t return_switch;			/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
-	switch_pos_t posctl_switch;			/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
-	switch_pos_t loiter_switch;			/**< loiter 2 position switch (optional): _MISSION_, LOITER */
-	switch_pos_t acro_switch;			/**< acro 2 position switch (optional): _MANUAL_, ACRO */
-	switch_pos_t offboard_switch;		/**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
+	switch_pos_t mode_switch;	/**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
+	switch_pos_t return_switch;	/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
+	switch_pos_t posctl_switch;	/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
+	switch_pos_t loiter_switch;	/**< loiter 2 position switch (optional): _MISSION_, LOITER */
+	switch_pos_t acro_switch;	/**< acro 2 position switch (optional): _MANUAL_, ACRO */
+	switch_pos_t offboard_switch;	/**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
 }; /**< manual control inputs */
 
 /**
-- 
cgit v1.2.3


From b1c6692f2028b616581efaf7a320a7cf7072a02f Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 12 Nov 2014 11:06:25 +1000
Subject: commander: removed unused definition

---
 src/modules/commander/commander.cpp | 2 --
 1 file changed, 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 46caddd46..149c0f60e 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -220,8 +220,6 @@ void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actua
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
-void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *status);
-
 transition_result_t set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoint_s *sp_man);
 
 void set_control_mode();
-- 
cgit v1.2.3


From f1058bdf0816cda718cee4d5a69c4ac3d53ea781 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 12 Nov 2014 11:07:27 +1000
Subject: manual_control_setpoint: wrong comment

---
 src/modules/uORB/topics/manual_control_setpoint.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index c0c33a629..50b7bd9e5 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -99,7 +99,7 @@ struct manual_control_setpoint_s {
 	switch_pos_t loiter_switch;	/**< loiter 2 position switch (optional): _MISSION_, LOITER */
 	switch_pos_t acro_switch;	/**< acro 2 position switch (optional): _MANUAL_, ACRO */
 	switch_pos_t offboard_switch;	/**< offboard 2 position switch (optional): _NORMAL_, OFFBOARD */
-}; /**< manual control inputs */
+};
 
 /**
  * @}
-- 
cgit v1.2.3


From 703fcec62d3430b75876d2c06183bbf4451f0dd6 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 12 Nov 2014 12:11:23 +1000
Subject: navigator: fix do jumps: after the repetitions are completed the
 current mission index is no longer invalid

---
 src/modules/navigator/mission.cpp | 21 ++++++++++++---------
 1 file changed, 12 insertions(+), 9 deletions(-)

(limited to 'src')

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 7fac69a61..840c9ec0e 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -371,7 +371,7 @@ Mission::set_mission_items()
 	} else {
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished");
 
 			/* use last setpoint for loiter */
 			_navigator->set_can_loiter_at_sp(true);
@@ -595,13 +595,14 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 	}
 
-	if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
-		/* mission item index out of bounds */
-		return false;
-	}
-
 	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
 	for (int i = 0; i < 10; i++) {
+
+		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
+			/* mission item index out of bounds */
+			return false;
+		}
+
 		const ssize_t len = sizeof(struct mission_item_s);
 
 		/* read mission item to temp storage first to not overwrite current mission item if data damaged */
@@ -630,7 +631,7 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 						/* not supposed to happen unless the datamanager can't access the
 						 * dataman */
 						mavlink_log_critical(_navigator->get_mavlink_fd(),
-								"ERROR DO JUMP waypoint could not be written");
+								     "ERROR DO JUMP waypoint could not be written");
 						return false;
 					}
 				}
@@ -639,8 +640,10 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 				*mission_index_ptr = mission_item_tmp.do_jump_mission_index;
 
 			} else {
-				mavlink_log_critical(_navigator->get_mavlink_fd(),
-						 "DO JUMP repetitions completed");
+				if (is_current) {
+					mavlink_log_critical(_navigator->get_mavlink_fd(),
+							     "DO JUMP repetitions completed");
+				}
 				/* no more DO_JUMPS, therefore just try to continue with next mission item */
 				(*mission_index_ptr)++;
 			}
-- 
cgit v1.2.3


From 08443205e90955c13532c79aa02a283a50cce25d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 12 Nov 2014 13:07:06 +1000
Subject: navigator: make DO_JUMPS survive power on resets

---
 src/modules/navigator/mission.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 840c9ec0e..cfe85c84e 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -627,7 +627,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 				if (is_current) {
 					(mission_item_tmp.do_jump_current_count)++;
 					/* save repeat count */
-					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_IN_FLIGHT_RESET, &mission_item_tmp, len) != len) {
+					if (dm_write(dm_item, *mission_index_ptr, DM_PERSIST_POWER_ON_RESET,
+					    &mission_item_tmp, len) != len) {
 						/* not supposed to happen unless the datamanager can't access the
 						 * dataman */
 						mavlink_log_critical(_navigator->get_mavlink_fd(),
-- 
cgit v1.2.3


From 14b5732c6eff6998d013c0816e983320906c462d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 12 Nov 2014 13:26:10 +1000
Subject: px4flow: small verbose output fix

---
 src/drivers/px4flow/px4flow.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 6c68636c6..9e1ecb61e 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -76,7 +76,7 @@
 #define PX4FLOW_BUS 			PX4_I2C_BUS_EXPANSION
 #define I2C_FLOW_ADDRESS 		0x42 //* 7-bit address. 8-bit address is 0x84
 //range 0x42 - 0x49
- 
+
 /* PX4FLOW Registers addresses */
 #define PX4FLOW_REG	0x00		/* Measure Register */
 
@@ -443,7 +443,7 @@ PX4FLOW::measure()
 	if (OK != ret) {
 		perf_count(_comms_errors);
 		log("i2c::transfer returned %d", ret);
-		printf("i2c::transfer flow returned %d");
+		printf("i2c::transfer flow returned %d", ret);
 		return ret;
 	}
 
-- 
cgit v1.2.3


From 7bc9a6257731a741ff309f060f6cf87c33c4a266 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Wed, 12 Nov 2014 09:52:35 +0100
Subject: code style, warnings

---
 src/modules/uavcan/actuators/esc.cpp   | 13 +++++++------
 src/modules/uavcan/uavcan_main.cpp     |  9 ++++-----
 src/systemcmds/motor_test/motor_test.c | 19 ++++++++-----------
 3 files changed, 19 insertions(+), 22 deletions(-)

(limited to 'src')

diff --git a/src/modules/uavcan/actuators/esc.cpp b/src/modules/uavcan/actuators/esc.cpp
index fbd4f0bcd..1d23099f3 100644
--- a/src/modules/uavcan/actuators/esc.cpp
+++ b/src/modules/uavcan/actuators/esc.cpp
@@ -113,8 +113,7 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 			msg.cmd.push_back(static_cast<int>(scaled));
 
 			_esc_status.esc[i].esc_setpoint_raw = abs(static_cast<int>(scaled));
-		}
-		else {
+		} else {
 			msg.cmd.push_back(static_cast<unsigned>(0));
 		}
 	}
@@ -128,18 +127,20 @@ void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
 
 void UavcanEscController::arm_all_escs(bool arm)
 {
-	if (arm)
+	if (arm) {
 		_armed_mask = -1;
-	else
+	} else {
 		_armed_mask = 0;
+	}
 }
 
 void UavcanEscController::arm_single_esc(int num, bool arm)
 {
-	if (arm)
+	if (arm) {
 		_armed_mask = MOTOR_BIT(num);
-	else
+	} else {
 		_armed_mask = 0;
+	}
 }
 
 void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 653d4f98c..2c543462e 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -346,13 +346,12 @@ int UavcanNode::run()
 
 			// can we mix?
 			if (_test_in_progress) {
-				float outputs[NUM_ACTUATOR_OUTPUTS] = {};
-				outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
+				float test_outputs[NUM_ACTUATOR_OUTPUTS] = {};
+				test_outputs[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
 
 				// Output to the bus
-				_esc_controller.update_outputs(outputs, NUM_ACTUATOR_OUTPUTS);
-			}
-			else if (controls_updated && (_mixers != nullptr)) {
+				_esc_controller.update_outputs(test_outputs, NUM_ACTUATOR_OUTPUTS);
+			} else if (controls_updated && (_mixers != nullptr)) {
 
 				// XXX one output group has 8 outputs max,
 				// but this driver could well serve multiple groups.
diff --git a/src/systemcmds/motor_test/motor_test.c b/src/systemcmds/motor_test/motor_test.c
index 079f99674..087699b05 100644
--- a/src/systemcmds/motor_test/motor_test.c
+++ b/src/systemcmds/motor_test/motor_test.c
@@ -67,19 +67,15 @@ void motor_test(unsigned channel, float value)
 	_test_motor.timestamp = hrt_absolute_time();
 	_test_motor.value = value;
 
-        if (_test_motor_pub > 0) {
-	    /* publish armed state */
-            orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
-        } else {
-            /* advertise and publish */
-            _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
-        }
+        _test_motor_pub = orb_advertise(ORB_ID(test_motor), &_test_motor);
+        orb_publish(ORB_ID(test_motor), _test_motor_pub, &_test_motor);
 }
 
 static void usage(const char *reason)
 {
-	if (reason != NULL)
+	if (reason != NULL) {
 		warnx("%s", reason);
+	}
 
 	errx(1,
 		"usage:\n"
@@ -90,8 +86,9 @@ static void usage(const char *reason)
 
 int motor_test_main(int argc, char *argv[])
 {
-	unsigned long channel, lval;
-	float value;
+	unsigned long channel = 0;
+	unsigned long lval;
+	float value = 0.0f;
 	int ch;
 
 	if (argc != 5) {
@@ -122,7 +119,7 @@ int motor_test_main(int argc, char *argv[])
 
 	motor_test(channel, value);
 
-	printf("motor %d set to %.2f\n", channel, value);
+	printf("motor %d set to %.2f\n", channel, (double)value);
 
 	exit(0);
 }
-- 
cgit v1.2.3


From 64de403b5f2aa4af57918510451373424c162b68 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 12 Nov 2014 20:06:49 +1000
Subject: navigator: trying to improve a comment

---
 src/modules/navigator/mission.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index cfe85c84e..d0611b9e5 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -595,7 +595,8 @@ Mission::read_mission_item(bool onboard, bool is_current, struct mission_item_s
 		dm_item = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 	}
 
-	/* repeat several to get the mission item because we might have to follow multiple DO_JUMPS */
+	/* Repeat this several times in case there are several DO JUMPS that we need to follow along, however, after
+	 * 10 iterations we have to assume that the DO JUMPS are probably cycling and give up. */
 	for (int i = 0; i < 10; i++) {
 
 		if (*mission_index_ptr < 0 || *mission_index_ptr >= (int)mission->count) {
-- 
cgit v1.2.3


From afcbab7fba84cf3d3d016a63fbca405ade15889a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 12 Nov 2014 13:37:58 +0100
Subject: PX4FLOW driver: Prevent sensor from spamming the system on errors.
 Use the perf system call to look at error counters.

---
 src/drivers/px4flow/px4flow.cpp | 12 +++++-------
 1 file changed, 5 insertions(+), 7 deletions(-)

(limited to 'src')

diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 9e1ecb61e..804027b05 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -200,7 +200,7 @@ PX4FLOW::PX4FLOW(int bus, int address) :
 	_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
 {
 	// enable debug() calls
-	_debug_enabled = true;
+	_debug_enabled = false;
 
 	// work_cancel in the dtor will explode if we don't do this...
 	memset(&_work, 0, sizeof(_work));
@@ -240,7 +240,7 @@ PX4FLOW::init()
 	_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &zero_report);
 
 	if (_px4flow_topic < 0) {
-		debug("failed to create px4flow object. Did you start uOrb?");
+		warnx("failed to create px4flow object. Did you start uOrb?");
 	}
 
 	ret = OK;
@@ -442,8 +442,6 @@ PX4FLOW::measure()
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		log("i2c::transfer returned %d", ret);
-		printf("i2c::transfer flow returned %d", ret);
 		return ret;
 	}
 
@@ -465,7 +463,7 @@ PX4FLOW::collect()
 	ret = transfer(nullptr, 0, &val[0], 22);
 
 	if (ret < 0) {
-		log("error reading from sensor: %d", ret);
+		debug("error reading from sensor: %d", ret);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);
 		return ret;
@@ -565,7 +563,7 @@ PX4FLOW::cycle()
 
 		/* perform collection */
 		if (OK != collect()) {
-			log("collection error");
+			debug("collection error");
 			/* restart the measurement state machine */
 			start();
 			return;
@@ -592,7 +590,7 @@ PX4FLOW::cycle()
 
 	/* measurement phase */
 	if (OK != measure()) {
-		log("measure error");
+		debug("measure error");
 	}
 
 	/* next phase is collection */
-- 
cgit v1.2.3


From 826fd3f4cdf492be1e5b05f7da4493ff10118876 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 13 Nov 2014 09:43:51 +1000
Subject: commander: reordering navigation states to match enum

---
 src/modules/commander/commander.cpp | 153 +++++++++++++++++++-----------------
 1 file changed, 83 insertions(+), 70 deletions(-)

(limited to 'src')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 7917bc9f2..3307c4bfc 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2201,18 +2201,6 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_ACRO:
-		control_mode.flag_control_manual_enabled = true;
-		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_rates_enabled = true;
-		control_mode.flag_control_attitude_enabled = false;
-		control_mode.flag_control_altitude_enabled = false;
-		control_mode.flag_control_climb_rate_enabled = false;
-		control_mode.flag_control_position_enabled = false;
-		control_mode.flag_control_velocity_enabled = false;
-		control_mode.flag_control_termination_enabled = false;
-		break;
-
 	case NAVIGATION_STATE_ALTCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
@@ -2225,64 +2213,6 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-	case NAVIGATION_STATE_OFFBOARD:
-		control_mode.flag_control_manual_enabled = false;
-		control_mode.flag_control_auto_enabled = false;
-		control_mode.flag_control_offboard_enabled = true;
-
-		switch (sp_offboard.mode) {
-		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_force_enabled = true;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-			break;
-
-		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
-		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
-			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true;
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = true;
-			control_mode.flag_control_velocity_enabled = true;
-			//XXX: the flags could depend on sp_offboard.ignore
-			break;
-
-		default:
-			control_mode.flag_control_rates_enabled = false;
-			control_mode.flag_control_attitude_enabled = false;
-			control_mode.flag_control_altitude_enabled = false;
-			control_mode.flag_control_climb_rate_enabled = false;
-			control_mode.flag_control_position_enabled = false;
-			control_mode.flag_control_velocity_enabled = false;
-		}
-
-		break;
-
 	case NAVIGATION_STATE_POSCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;
@@ -2323,6 +2253,19 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case NAVIGATION_STATE_ACRO:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
+
 	case NAVIGATION_STATE_LAND:
 		control_mode.flag_control_manual_enabled = false;
 		control_mode.flag_control_auto_enabled = true;
@@ -2336,6 +2279,19 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case NAVIGATION_STATE_DESCEND:
+		/* TODO: check if this makes sense */
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = true;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = true;
+		control_mode.flag_control_termination_enabled = false;
+		break;
+
 	case NAVIGATION_STATE_TERMINATION:
 		/* disable all controllers on termination */
 		control_mode.flag_control_manual_enabled = false;
@@ -2349,6 +2305,63 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = true;
 		break;
 
+	case NAVIGATION_STATE_OFFBOARD:
+		control_mode.flag_control_manual_enabled = false;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_offboard_enabled = true;
+
+		switch (sp_offboard.mode) {
+		case OFFBOARD_CONTROL_MODE_DIRECT_RATES:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_force_enabled = true;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
+
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = true;
+			control_mode.flag_control_altitude_enabled = true;
+			control_mode.flag_control_climb_rate_enabled = true;
+			control_mode.flag_control_position_enabled = true;
+			control_mode.flag_control_velocity_enabled = true;
+			//XXX: the flags could depend on sp_offboard.ignore
+			break;
+
+		default:
+			control_mode.flag_control_rates_enabled = false;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+		}
+		break;
+
 	default:
 		break;
 	}
-- 
cgit v1.2.3


From 00cc2d55111fe6dcf03fc847aa6e65204e4a0a2c Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 13 Nov 2014 09:44:17 +1000
Subject: commander: added navstate RCRECOVER which was missing

---
 src/modules/commander/commander.cpp | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 3307c4bfc..5c4ad26f4 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2225,9 +2225,11 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+
 	case NAVIGATION_STATE_AUTO_MISSION:
 	case NAVIGATION_STATE_AUTO_LOITER:
 	case NAVIGATION_STATE_AUTO_RTL:
+	case NAVIGATION_STATE_AUTO_RCRECOVER:
 	case NAVIGATION_STATE_AUTO_RTGS:
 	case NAVIGATION_STATE_AUTO_LANDENGFAIL:
 		control_mode.flag_control_manual_enabled = false;
-- 
cgit v1.2.3


From db3f2fc94621ee13c7ec22d5fef1dd5ae74a0742 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Thu, 13 Nov 2014 09:46:12 +1000
Subject: commander: whitespace

---
 src/modules/commander/commander.cpp | 1 -
 1 file changed, 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 5c4ad26f4..ec173c12b 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -2225,7 +2225,6 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
-
 	case NAVIGATION_STATE_AUTO_MISSION:
 	case NAVIGATION_STATE_AUTO_LOITER:
 	case NAVIGATION_STATE_AUTO_RTL:
-- 
cgit v1.2.3


From 6a543b1d0154d7da9ddb4123df1479cf01aa88bc Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Thu, 13 Nov 2014 10:45:32 +0100
Subject: build fix

---
 src/modules/uORB/topics/test_motor.h | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/uORB/topics/test_motor.h b/src/modules/uORB/topics/test_motor.h
index c880fe099..2dd90e69d 100644
--- a/src/modules/uORB/topics/test_motor.h
+++ b/src/modules/uORB/topics/test_motor.h
@@ -57,7 +57,7 @@
 struct test_motor_s {
 	uint64_t 	timestamp;				/**< output timestamp in us since system boot */
 	unsigned 	motor_number;				/**< number of motor to spin */
-	float		value;					/**< output power, range [0..1]
+	float		value;					/**< output power, range [0..1] */
 };
 
 /**
-- 
cgit v1.2.3


From 4d489ef7f4c7626a86b700b6d7144e35000ce387 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Tue, 11 Nov 2014 11:19:15 +0100
Subject: UAVCAN: improve mag compatibility

---
 src/modules/uavcan/sensors/mag.cpp | 3 +++
 1 file changed, 3 insertions(+)

(limited to 'src')

diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 0d9ea08c5..4f56038b5 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -74,6 +74,9 @@ int UavcanMagnetometerBridge::init()
 int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
+	case SENSORIOCSQUEUEDEPTH: {
+		return OK;			// Pretend that this stuff is supported to keep APM happy
+	}
 	case MAGIOCSSCALE: {
 		std::memcpy(&_scale, reinterpret_cast<const void*>(arg), sizeof(_scale));
 		return 0;
-- 
cgit v1.2.3


From 0fa622f22b90048ad366ac304c2c9339a5c376d3 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Tue, 11 Nov 2014 11:20:03 +0100
Subject: UAVCAN: declare mag external again to allow different rotation than
 the internal mag

---
 src/modules/uavcan/sensors/mag.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 4f56038b5..4d473291c 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -89,7 +89,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 		return 0;           // Nothing to do
 	}
 	case MAGIOCGEXTERNAL: {
-		return 0;           // We don't want anyone to transform the coordinate frame, so we declare it onboard
+		return 1;           // We don't want anyone to transform the coordinate frame, so we declare it onboard
 	}
 	case MAGIOCSSAMPLERATE: {
 		return 0;           // Pretend that this stuff is supported to keep the sensor app happy
-- 
cgit v1.2.3


From 16d74e3c31b9c2cf4f2de254956a58885b5c1fbf Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Wed, 12 Nov 2014 13:14:05 +0100
Subject: UACVAN: add read()-style interface to mag device

---
 src/modules/uavcan/sensors/mag.cpp | 48 +++++++++++++++++++++++++++++---------
 src/modules/uavcan/sensors/mag.hpp |  2 ++
 2 files changed, 39 insertions(+), 11 deletions(-)

(limited to 'src')

diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 4d473291c..a529e7e75 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -37,6 +37,8 @@
 
 #include "mag.hpp"
 
+#include <systemlib/err.h>
+
 static const orb_id_t MAG_TOPICS[3] = {
 	ORB_ID(sensor_mag0),
 	ORB_ID(sensor_mag1),
@@ -71,6 +73,30 @@ int UavcanMagnetometerBridge::init()
 	return 0;
 }
 
+ssize_t UavcanMagnetometerBridge::read(struct file *filp, char *buffer, size_t buflen)
+{
+	static uint64_t last_read = 0;
+	struct mag_report *mag_buf = reinterpret_cast<struct mag_report *>(buffer);
+
+	/* buffer must be large enough */
+	unsigned count = buflen / sizeof(struct mag_report);
+	if (count < 1) {
+		return -ENOSPC;
+	}
+
+	if (last_read < _report.timestamp) {
+		/* copy report */
+		lock();
+		*mag_buf = _report;
+		last_read = _report.timestamp;
+		unlock();
+		return sizeof(struct mag_report);
+	} else {
+		/* no new data available, warn caller */
+		return -EAGAIN;
+	}
+}
+
 int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
@@ -89,7 +115,7 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 		return 0;           // Nothing to do
 	}
 	case MAGIOCGEXTERNAL: {
-		return 1;           // We don't want anyone to transform the coordinate frame, so we declare it onboard
+		return 1;           // declare it external rise it's priority and to allow for correct orientation compensation
 	}
 	case MAGIOCSSAMPLERATE: {
 		return 0;           // Pretend that this stuff is supported to keep the sensor app happy
@@ -111,18 +137,18 @@ int UavcanMagnetometerBridge::ioctl(struct file *filp, int cmd, unsigned long ar
 
 void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg)
 {
-	auto report = ::mag_report();
-
-	report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
+	lock();
+	_report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
 
-	report.timestamp = msg.getUtcTimestamp().toUSec();
-	if (report.timestamp == 0) {
-		report.timestamp = msg.getMonotonicTimestamp().toUSec();
+	_report.timestamp = msg.getUtcTimestamp().toUSec();
+	if (_report.timestamp == 0) {
+		_report.timestamp = msg.getMonotonicTimestamp().toUSec();
 	}
 
-	report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
-	report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
-	report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+	_report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
+	_report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
+	_report.z = (msg.magnetic_field[2] - _scale.z_offset) * _scale.z_scale;
+	unlock();
 
-	publish(msg.getSrcNodeID().get(), &report);
+	publish(msg.getSrcNodeID().get(), &_report);
 }
diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp
index 6d413a8f7..74077d883 100644
--- a/src/modules/uavcan/sensors/mag.hpp
+++ b/src/modules/uavcan/sensors/mag.hpp
@@ -54,6 +54,7 @@ public:
 	int init() override;
 
 private:
+	ssize_t	read(struct file *filp, char *buffer, size_t buflen);
 	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
 
 	void mag_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::ahrs::Magnetometer> &msg);
@@ -65,4 +66,5 @@ private:
 
 	uavcan::Subscriber<uavcan::equipment::ahrs::Magnetometer, MagCbBinder> _sub_mag;
 	mag_scale _scale = {};
+	mag_report _report =  {};
 };
-- 
cgit v1.2.3


From e026324784409bd16a561544cb608e7089d500a0 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Thu, 13 Nov 2014 16:58:41 +0100
Subject: UAVCAN: fix mag report timestamp

---
 src/modules/uavcan/sensors/mag.cpp | 7 ++-----
 1 file changed, 2 insertions(+), 5 deletions(-)

(limited to 'src')

diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index a529e7e75..5cbb96433 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -37,6 +37,7 @@
 
 #include "mag.hpp"
 
+#include <drivers/drv_hrt.h>
 #include <systemlib/err.h>
 
 static const orb_id_t MAG_TOPICS[3] = {
@@ -139,11 +140,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<ua
 {
 	lock();
 	_report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
-
-	_report.timestamp = msg.getUtcTimestamp().toUSec();
-	if (_report.timestamp == 0) {
-		_report.timestamp = msg.getMonotonicTimestamp().toUSec();
-	}
+	_report.timestamp = hrt_absolute_time();
 
 	_report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
 	_report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
-- 
cgit v1.2.3


From 4bcb2697415009279bd481eb9d22f015846147f1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 13:48:49 +0100
Subject: Add capability to read chip ID and revision from ver command

---
 src/systemcmds/ver/ver.c | 85 +++++++++++++++++++++++++++++++++++++++++++++++-
 1 file changed, 84 insertions(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 9ae080ee2..b11b593c3 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -53,13 +53,38 @@ static const char sz_ver_bdate_str[] = "bdate";
 static const char sz_ver_gcc_str[] 	= "gcc";
 static const char sz_ver_all_str[] 	= "all";
 
+#ifdef CONFIG_ARCH_CHIP_STM32
+#include <up_arch.h>
+
+static const char mcu_ver_str[]		= "mcu";
+
+#define DBGMCU_IDCODE	0xE0042000
+
+#define STM32F40x_41x	0x413
+#define STM32F42x_43x	0x419
+
+#define REVID_MASK	0xFFFF0000
+#define DEVID_MASK	0xFFF
+
+/* magic numbers from reference manual */
+enum STM32F4_REV {
+	STM32F4_REV_A = 0x1000,
+	STM32F4_REV_Z = 0x1001,
+	STM32F4_REV_Y = 0x1003,
+	STM32F4_REV_1 = 0x1007,
+	STM32F4_REV_3 = 0x2001
+};
+#else
+#error stm32
+#endif
+
 static void usage(const char *reason)
 {
 	if (reason != NULL) {
 		printf("%s\n", reason);
 	}
 
-	printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n");
+	printf("usage: ver {hw|hwcmp|git|bdate|gcc|all|mcu}\n\n");
 }
 
 __EXPORT int ver_main(int argc, char *argv[]);
@@ -107,6 +132,64 @@ int ver_main(int argc, char *argv[])
 				printf("GCC toolchain: %s\n", __VERSION__);
 				ret = 0;
 
+#ifdef CONFIG_ARCH_CHIP_STM32
+			} else if (!strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) {
+				uint32_t abc = getreg32(DBGMCU_IDCODE);
+
+				uint32_t chip_version = abc & DEVID_MASK;
+				enum STM32F4_REV revid = (abc & REVID_MASK) >> 16;
+
+				printf("CHIP TYPE: ");
+
+				switch (revid) {
+				case STM32F40x_41x:
+					printf("STM32F40x");
+					break;
+				case STM32F42x_43x:
+					printf("STM32F42x");
+					break;
+				default:
+					printf("STM32F???");
+					break;
+				}
+
+				char rev;
+
+				switch (chip_version) {
+
+					case STM32F4_REV_A:
+						rev = 'A';
+						break;
+					case STM32F4_REV_Z:
+						rev = 'Z';
+						break;
+					case STM32F4_REV_Y:
+						rev = 'Y';
+						break;
+					case STM32F4_REV_1:
+						rev = '1';
+						break;
+					case STM32F4_REV_3:
+						rev = '3';
+						break;
+					default:
+						rev = '?';
+						break;
+				}
+
+				printf("\nHW REV: %c\n", rev);
+
+				if (rev < STM32F4_REV_3) {
+					printf("\n\nWARNING   WARNING   WARNING!\n"
+						"Revision %c has a silicon errata\n"
+						"on USB connectivity combined with\n"
+						"flash bank #2. This device can only\n"
+						"utilize a maximum of 1MB flash safely!\n"
+						"http://px4.io/help/errata\n", rev);
+				}
+				ret = 0;
+#endif
+
 			} else {
 				errx(1, "unknown command.\n");
 			}
-- 
cgit v1.2.3


From a7eaf07f4f9d6df54b7602c07ce7fbdf2db700f9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 13:49:48 +0100
Subject: Motor test: optimize for size

---
 src/systemcmds/motor_test/module.mk | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/systemcmds/motor_test/module.mk b/src/systemcmds/motor_test/module.mk
index eb36d2ded..261428ef9 100644
--- a/src/systemcmds/motor_test/module.mk
+++ b/src/systemcmds/motor_test/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	 = motor_test
 SRCS		 = motor_test.c
 
 MODULE_STACKSIZE = 4096
+
+MAXOPTIMIZATION	 = -Os
-- 
cgit v1.2.3


From 3d2a5bae51763e5a542506383c9d97f95fc7d1ef Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 13:51:16 +0100
Subject: Board drivers: Optimize for size

---
 src/drivers/boards/aerocore/module.mk  | 2 ++
 src/drivers/boards/px4fmu-v1/module.mk | 2 ++
 src/drivers/boards/px4fmu-v2/module.mk | 2 ++
 src/drivers/boards/px4io-v1/module.mk  | 2 ++
 src/drivers/boards/px4io-v2/module.mk  | 2 ++
 5 files changed, 10 insertions(+)

(limited to 'src')

diff --git a/src/drivers/boards/aerocore/module.mk b/src/drivers/boards/aerocore/module.mk
index b53fe0a29..0a2d91009 100644
--- a/src/drivers/boards/aerocore/module.mk
+++ b/src/drivers/boards/aerocore/module.mk
@@ -6,3 +6,5 @@ SRCS		 = aerocore_init.c \
 		   aerocore_pwm_servo.c \
 		   aerocore_spi.c \
 		   aerocore_led.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4fmu-v1/module.mk b/src/drivers/boards/px4fmu-v1/module.mk
index 66b776917..5e1a27d5a 100644
--- a/src/drivers/boards/px4fmu-v1/module.mk
+++ b/src/drivers/boards/px4fmu-v1/module.mk
@@ -8,3 +8,5 @@ SRCS		 = px4fmu_can.c \
 		   px4fmu_spi.c \
 		   px4fmu_usb.c \
 		   px4fmu_led.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4fmu-v2/module.mk b/src/drivers/boards/px4fmu-v2/module.mk
index 99d37eeca..103232b0c 100644
--- a/src/drivers/boards/px4fmu-v2/module.mk
+++ b/src/drivers/boards/px4fmu-v2/module.mk
@@ -8,3 +8,5 @@ SRCS		 = px4fmu_can.c \
 		   px4fmu_spi.c \
 		   px4fmu_usb.c \
 		   px4fmu2_led.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4io-v1/module.mk b/src/drivers/boards/px4io-v1/module.mk
index 2601a1c15..a7a14dd07 100644
--- a/src/drivers/boards/px4io-v1/module.mk
+++ b/src/drivers/boards/px4io-v1/module.mk
@@ -4,3 +4,5 @@
 
 SRCS	= px4io_init.c \
 	  px4io_pwm_servo.c
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/boards/px4io-v2/module.mk b/src/drivers/boards/px4io-v2/module.mk
index 85f94e8be..3f0e9a0b3 100644
--- a/src/drivers/boards/px4io-v2/module.mk
+++ b/src/drivers/boards/px4io-v2/module.mk
@@ -4,3 +4,5 @@
 
 SRCS		= px4iov2_init.c \
 		  px4iov2_pwm_servo.c
+
+MAXOPTIMIZATION	 = -Os
-- 
cgit v1.2.3


From c0f34dff2605381afacbec2bc1eba6b648daddd2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 13:51:33 +0100
Subject: STM32 drivers: Optimize for size

---
 src/drivers/stm32/adc/module.mk        | 2 ++
 src/drivers/stm32/module.mk            | 2 ++
 src/drivers/stm32/tone_alarm/module.mk | 2 ++
 3 files changed, 6 insertions(+)

(limited to 'src')

diff --git a/src/drivers/stm32/adc/module.mk b/src/drivers/stm32/adc/module.mk
index 48620feea..38ea490a0 100644
--- a/src/drivers/stm32/adc/module.mk
+++ b/src/drivers/stm32/adc/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= adc
 SRCS		= adc.cpp
 
 INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/stm32/module.mk b/src/drivers/stm32/module.mk
index bb751c7f6..54428e270 100644
--- a/src/drivers/stm32/module.mk
+++ b/src/drivers/stm32/module.mk
@@ -41,3 +41,5 @@ SRCS		= drv_hrt.c \
 		  drv_pwm_servo.c
 
 INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/stm32/tone_alarm/module.mk b/src/drivers/stm32/tone_alarm/module.mk
index 827cf30b2..25a194ef6 100644
--- a/src/drivers/stm32/tone_alarm/module.mk
+++ b/src/drivers/stm32/tone_alarm/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND	= tone_alarm
 SRCS		= tone_alarm.cpp
 
 INCLUDE_DIRS	+= $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/common
+
+MAXOPTIMIZATION	 = -Os
-- 
cgit v1.2.3


From f7f54062439f0b1c1208d0778f992f7052e1518b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 13:51:49 +0100
Subject: FMU driver: optimize for size

---
 src/drivers/px4fmu/module.mk | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/drivers/px4fmu/module.mk b/src/drivers/px4fmu/module.mk
index a60f1a434..a06323a52 100644
--- a/src/drivers/px4fmu/module.mk
+++ b/src/drivers/px4fmu/module.mk
@@ -8,3 +8,5 @@ SRCS		 = fmu.cpp
 MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
-- 
cgit v1.2.3


From a12c98ba63c12ba08f1fb0f66df7b7079e444e1d Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 13:52:08 +0100
Subject: Sensors: Optimize for size

---
 src/modules/sensors/module.mk | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/modules/sensors/module.mk b/src/modules/sensors/module.mk
index 5b1bc5e86..dfbba92d9 100644
--- a/src/modules/sensors/module.mk
+++ b/src/modules/sensors/module.mk
@@ -42,3 +42,5 @@ SRCS		= sensors.cpp \
 		  sensor_params.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
-- 
cgit v1.2.3


From a869105ba23394cfabb0a0000dd3b7fa3dc9fdb1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 13:52:34 +0100
Subject: Systemlib: Optimize for size

---
 src/modules/systemlib/module.mk | 1 +
 1 file changed, 1 insertion(+)

(limited to 'src')

diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index 147903aa0..5c4f2ec41 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -55,3 +55,4 @@ SRCS		 = err.c \
 		   pwm_limit/pwm_limit.c \
 		   circuit_breaker.c
 
+MAXOPTIMIZATION	 = -Os
-- 
cgit v1.2.3


From 54f296ce9d8245845a7a0bf32f5045a44aab40c7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 13:54:01 +0100
Subject: Sensor drivers: Optimize for size

---
 src/drivers/hmc5883/module.mk | 2 ++
 src/drivers/l3gd20/module.mk  | 2 ++
 src/drivers/lsm303d/module.mk | 2 ++
 src/drivers/mpu6000/module.mk | 2 ++
 4 files changed, 8 insertions(+)

(limited to 'src')

diff --git a/src/drivers/hmc5883/module.mk b/src/drivers/hmc5883/module.mk
index 5daa01dc5..be2ee7276 100644
--- a/src/drivers/hmc5883/module.mk
+++ b/src/drivers/hmc5883/module.mk
@@ -42,3 +42,5 @@ SRCS		= hmc5883.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/l3gd20/module.mk b/src/drivers/l3gd20/module.mk
index 5630e7aec..3d64d62be 100644
--- a/src/drivers/l3gd20/module.mk
+++ b/src/drivers/l3gd20/module.mk
@@ -8,3 +8,5 @@ SRCS		 = l3gd20.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/lsm303d/module.mk b/src/drivers/lsm303d/module.mk
index b4f3974f4..0421eb113 100644
--- a/src/drivers/lsm303d/module.mk
+++ b/src/drivers/lsm303d/module.mk
@@ -8,3 +8,5 @@ SRCS		 = lsm303d.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/mpu6000/module.mk b/src/drivers/mpu6000/module.mk
index 5b4893b12..da9fcc0fc 100644
--- a/src/drivers/mpu6000/module.mk
+++ b/src/drivers/mpu6000/module.mk
@@ -42,3 +42,5 @@ SRCS		= mpu6000.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
-- 
cgit v1.2.3


From 32313a13dd1f98426f83b99310fc22b5adced37c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 13:54:16 +0100
Subject: PX4IO driver: Optimize for size

---
 src/drivers/px4io/module.mk | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/drivers/px4io/module.mk b/src/drivers/px4io/module.mk
index 5b838fb75..924283356 100644
--- a/src/drivers/px4io/module.mk
+++ b/src/drivers/px4io/module.mk
@@ -48,3 +48,5 @@ INCLUDE_DIRS    += $(NUTTX_SRC)/arch/arm/src/stm32 $(NUTTX_SRC)/arch/arm/src/com
 MODULE_STACKSIZE = 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
-- 
cgit v1.2.3


From 42575bbc3763fe2b9e2d3e5b074ec74e4fc57f5a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 13:54:38 +0100
Subject: Bottle drop: Optimize for size

---
 src/modules/bottle_drop/module.mk | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk
index 6b18fff55..960228879 100644
--- a/src/modules/bottle_drop/module.mk
+++ b/src/modules/bottle_drop/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND		= bottle_drop
 
 SRCS			= bottle_drop.cpp \
 			  bottle_drop_params.c
+
+MAXOPTIMIZATION	 = -Os
-- 
cgit v1.2.3


From a7bc52754de52ccbf0f958c20ce31408a8b4372e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 13:54:54 +0100
Subject: SDLOG2: Optimize for size

---
 src/modules/sdlog2/module.mk | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/modules/sdlog2/module.mk b/src/modules/sdlog2/module.mk
index a28d43e72..d4a00af39 100644
--- a/src/modules/sdlog2/module.mk
+++ b/src/modules/sdlog2/module.mk
@@ -43,3 +43,5 @@ SRCS = sdlog2.c \
        logbuffer.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
-- 
cgit v1.2.3


From 7b962e7d1bb6b60633c116b9d5395b24e6658664 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 14:17:24 +0100
Subject: Version command optimization

---
 src/systemcmds/ver/ver.c | 4 +---
 1 file changed, 1 insertion(+), 3 deletions(-)

(limited to 'src')

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index b11b593c3..9d308bc3e 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -182,9 +182,7 @@ int ver_main(int argc, char *argv[])
 				if (rev < STM32F4_REV_3) {
 					printf("\n\nWARNING   WARNING   WARNING!\n"
 						"Revision %c has a silicon errata\n"
-						"on USB connectivity combined with\n"
-						"flash bank #2. This device can only\n"
-						"utilize a maximum of 1MB flash safely!\n"
+						"This device can only utilize a maximum of 1MB flash safely!\n"
 						"http://px4.io/help/errata\n", rev);
 				}
 				ret = 0;
-- 
cgit v1.2.3


From 2a37d274b142f49e250265eb14821d0b4f7e51cf Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 15:51:55 +0100
Subject: Systemlib: Add MCU version command. ver commandline tool: Add support
 for MCU version command

---
 src/modules/systemlib/mcu_version.c | 109 ++++++++++++++++++++++++++++++++++++
 src/modules/systemlib/mcu_version.h |  52 +++++++++++++++++
 src/modules/systemlib/module.mk     |   4 +-
 src/systemcmds/ver/ver.c            |  88 ++++++-----------------------
 4 files changed, 179 insertions(+), 74 deletions(-)
 create mode 100644 src/modules/systemlib/mcu_version.c
 create mode 100644 src/modules/systemlib/mcu_version.h

(limited to 'src')

diff --git a/src/modules/systemlib/mcu_version.c b/src/modules/systemlib/mcu_version.c
new file mode 100644
index 000000000..4bcf95784
--- /dev/null
+++ b/src/modules/systemlib/mcu_version.c
@@ -0,0 +1,109 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 mcu_version.c
+ * 
+ * Read out the microcontroller version from the board
+ *
+ * @author Lorenz Meier <lorenz@px4.io>
+ *
+ */
+
+#include "mcu_version.h"
+
+#include <nuttx/config.h>
+
+#ifdef CONFIG_ARCH_CHIP_STM32
+#include <up_arch.h>
+
+#define DBGMCU_IDCODE	0xE0042000
+
+#define STM32F40x_41x	0x413
+#define STM32F42x_43x	0x419
+
+#define REVID_MASK	0xFFFF0000
+#define DEVID_MASK	0xFFF
+
+#endif
+
+
+
+int mcu_version(char* rev, char** revstr)
+{
+#ifdef CONFIG_ARCH_CHIP_STM32
+	uint32_t abc = getreg32(DBGMCU_IDCODE);
+
+	int32_t chip_version = abc & DEVID_MASK;
+	enum MCU_REV revid = (abc & REVID_MASK) >> 16;
+
+	switch (chip_version) {
+	case STM32F40x_41x:
+		*revstr = "STM32F40x";
+		break;
+	case STM32F42x_43x:
+		*revstr = "STM32F42x";
+		break;
+	default:
+		*revstr = "STM32F???";
+		break;
+	}
+
+	switch (revid) {
+
+		case MCU_REV_STM32F4_REV_A:
+			*rev = 'A';
+			break;
+		case MCU_REV_STM32F4_REV_Z:
+			*rev = 'Z';
+			break;
+		case MCU_REV_STM32F4_REV_Y:
+			*rev = 'Y';
+			break;
+		case MCU_REV_STM32F4_REV_1:
+			*rev = '1';
+			break;
+		case MCU_REV_STM32F4_REV_3:
+			*rev = '3';
+			break;
+		default:
+			*rev = '?';
+			revid = -1;
+			break;
+	}
+
+	return revid;
+#else
+	return -1;
+#endif
+}
diff --git a/src/modules/systemlib/mcu_version.h b/src/modules/systemlib/mcu_version.h
new file mode 100644
index 000000000..1b3d0aba9
--- /dev/null
+++ b/src/modules/systemlib/mcu_version.h
@@ -0,0 +1,52 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+/* magic numbers from reference manual */
+enum MCU_REV {
+	MCU_REV_STM32F4_REV_A = 0x1000,
+	MCU_REV_STM32F4_REV_Z = 0x1001,
+	MCU_REV_STM32F4_REV_Y = 0x1003,
+	MCU_REV_STM32F4_REV_1 = 0x1007,
+	MCU_REV_STM32F4_REV_3 = 0x2001
+};
+
+/**
+ * Reports the microcontroller version of the main CPU.
+ *
+ * @param rev The silicon revision character
+ * @param revstr The full chip name string
+ * @return The silicon revision / version number as integer
+ */
+__EXPORT int mcu_version(char* rev, char** revstr);
diff --git a/src/modules/systemlib/module.mk b/src/modules/systemlib/module.mk
index 147903aa0..233023e25 100644
--- a/src/modules/systemlib/module.mk
+++ b/src/modules/systemlib/module.mk
@@ -53,5 +53,5 @@ SRCS		 = err.c \
 		   otp.c \
 		   board_serial.c \
 		   pwm_limit/pwm_limit.c \
-		   circuit_breaker.c
-
+		   circuit_breaker.c \
+		   mcu_version.c
diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 9d308bc3e..62a7a5b92 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -44,6 +44,7 @@
 #include <string.h>
 #include <version/version.h>
 #include <systemlib/err.h>
+#include <systemlib/mcu_version.h>
 
 // string constants for version commands
 static const char sz_ver_hw_str[] 	= "hw";
@@ -52,32 +53,8 @@ static const char sz_ver_git_str[] 	= "git";
 static const char sz_ver_bdate_str[] = "bdate";
 static const char sz_ver_gcc_str[] 	= "gcc";
 static const char sz_ver_all_str[] 	= "all";
-
-#ifdef CONFIG_ARCH_CHIP_STM32
-#include <up_arch.h>
-
 static const char mcu_ver_str[]		= "mcu";
 
-#define DBGMCU_IDCODE	0xE0042000
-
-#define STM32F40x_41x	0x413
-#define STM32F42x_43x	0x419
-
-#define REVID_MASK	0xFFFF0000
-#define DEVID_MASK	0xFFF
-
-/* magic numbers from reference manual */
-enum STM32F4_REV {
-	STM32F4_REV_A = 0x1000,
-	STM32F4_REV_Z = 0x1001,
-	STM32F4_REV_Y = 0x1003,
-	STM32F4_REV_1 = 0x1007,
-	STM32F4_REV_3 = 0x2001
-};
-#else
-#error stm32
-#endif
-
 static void usage(const char *reason)
 {
 	if (reason != NULL) {
@@ -132,61 +109,28 @@ int ver_main(int argc, char *argv[])
 				printf("GCC toolchain: %s\n", __VERSION__);
 				ret = 0;
 
-#ifdef CONFIG_ARCH_CHIP_STM32
+
 			} else if (!strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) {
-				uint32_t abc = getreg32(DBGMCU_IDCODE);
-
-				uint32_t chip_version = abc & DEVID_MASK;
-				enum STM32F4_REV revid = (abc & REVID_MASK) >> 16;
-
-				printf("CHIP TYPE: ");
-
-				switch (revid) {
-				case STM32F40x_41x:
-					printf("STM32F40x");
-					break;
-				case STM32F42x_43x:
-					printf("STM32F42x");
-					break;
-				default:
-					printf("STM32F???");
-					break;
-				}
 
 				char rev;
+				char* revstr;
 
-				switch (chip_version) {
-
-					case STM32F4_REV_A:
-						rev = 'A';
-						break;
-					case STM32F4_REV_Z:
-						rev = 'Z';
-						break;
-					case STM32F4_REV_Y:
-						rev = 'Y';
-						break;
-					case STM32F4_REV_1:
-						rev = '1';
-						break;
-					case STM32F4_REV_3:
-						rev = '3';
-						break;
-					default:
-						rev = '?';
-						break;
-				}
+				int chip_version = mcu_version(&rev, &revstr);
+
+				if (chip_version < 0) {
+					printf("UNKNOWN MCU");
+					ret = 1;
 
-				printf("\nHW REV: %c\n", rev);
+				} else {
+					printf("MCU: %s, rev. %c\n", revstr, rev);
 
-				if (rev < STM32F4_REV_3) {
-					printf("\n\nWARNING   WARNING   WARNING!\n"
-						"Revision %c has a silicon errata\n"
-						"This device can only utilize a maximum of 1MB flash safely!\n"
-						"http://px4.io/help/errata\n", rev);
+					if (chip_version < MCU_REV_STM32F4_REV_3) {
+						printf("\n\nWARNING   WARNING   WARNING!\n"
+							"Revision %c has a silicon errata\n"
+							"This device can only utilize a maximum of 1MB flash safely!\n"
+							"http://px4.io/help/errata\n", rev);
+					}
 				}
-				ret = 0;
-#endif
 
 			} else {
 				errx(1, "unknown command.\n");
-- 
cgit v1.2.3


From e2c0ac3f700f003061e52fd61fd4770c982605c8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 16:40:29 +0100
Subject: airspeed driver: Be less chatty

---
 src/drivers/airspeed/airspeed.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/drivers/airspeed/airspeed.cpp b/src/drivers/airspeed/airspeed.cpp
index 293690d27..3a1e1b7b5 100644
--- a/src/drivers/airspeed/airspeed.cpp
+++ b/src/drivers/airspeed/airspeed.cpp
@@ -147,7 +147,7 @@ Airspeed::init()
 		_airspeed_pub = orb_advertise(ORB_ID(differential_pressure), &arp);
 
 		if (_airspeed_pub < 0)
-			warnx("failed to create airspeed sensor object. uORB started?");
+			warnx("uORB started?");
 	}
 
 	ret = OK;
-- 
cgit v1.2.3


From 6f71173f8c7a13565ef90a8fec44187ba392f038 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 16:40:42 +0100
Subject: ARDrone driver: be less chatty

---
 src/drivers/ardrone_interface/ardrone_interface.c  | 36 +++++++++-------------
 .../ardrone_interface/ardrone_motor_control.c      |  2 +-
 2 files changed, 15 insertions(+), 23 deletions(-)

(limited to 'src')

diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index e5bb772b3..9d2c1441d 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -89,8 +89,8 @@ static void
 usage(const char *reason)
 {
 	if (reason)
-		fprintf(stderr, "%s\n", reason);
-	fprintf(stderr, "usage: ardrone_interface {start|stop|status} [-d <UART>]\n\n");
+		warnx("%s\n", reason);
+	warnx("usage: {start|stop|status} [-d <UART>]\n\n");
 	exit(1);
 }
 
@@ -110,7 +110,7 @@ int ardrone_interface_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			printf("ardrone_interface already running\n");
+			warnx("already running\n");
 			/* this is not an error */
 			exit(0);
 		}
@@ -132,9 +132,9 @@ int ardrone_interface_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			printf("\tardrone_interface is running\n");
+			warnx("running");
 		} else {
-			printf("\tardrone_interface not started\n");
+			warnx("not started");
 		}
 		exit(0);
 	}
@@ -158,7 +158,7 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
 
 	/* Back up the original uart configuration to restore it after exit */
 	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR getting baudrate / termios config for %s: %d\n", uart_name, termios_state);
+		warnx("ERR: TCGETATTR %s: %d", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
@@ -171,14 +171,14 @@ static int ardrone_open_uart(char *uart_name, struct termios *uart_config_origin
 
 	/* Set baud rate */
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+		warnx("ERR: cfsetispeed %s: %d", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
 
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+		warnx("ERR: tcsetattr: %s", uart_name);
 		close(uart);
 		return -1;
 	}
@@ -192,9 +192,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 
 	char *device = "/dev/ttyS1";
 
-	/* welcome user */
-	printf("[ardrone_interface] Control started, taking over motors\n");
-
 	/* File descriptors */
 	int gpios;
 
@@ -237,7 +234,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	struct termios uart_config_original;
 
 	if (motor_test_mode) {
-		printf("[ardrone_interface] Motor test mode enabled, setting 10 %% thrust.\n");
+		warnx("setting 10 %% thrust.\n");
 	}
 
 	/* Led animation */
@@ -255,9 +252,6 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
-	printf("[ardrone_interface] Motors initialized - ready.\n");
-	fflush(stdout);
-
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
 	ardrone_write = ardrone_open_uart(device, &uart_config_original);
 
@@ -265,7 +259,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	gpios = ar_multiplexing_init();
 
 	if (ardrone_write < 0) {
-		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+		warnx("No UART, exiting.");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -273,7 +267,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	/* initialize motors */
 	if (OK != ar_init_motors(ardrone_write, gpios)) {
 		close(ardrone_write);
-		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
+		warnx("motor init fail");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -294,7 +288,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	gpios = ar_multiplexing_init();
 
 	if (ardrone_write < 0) {
-		fprintf(stderr, "[ardrone_interface] Failed opening AR.Drone UART, exiting.\n");
+		warnx("write fail");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -302,7 +296,7 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	/* initialize motors */
 	if (OK != ar_init_motors(ardrone_write, gpios)) {
 		close(ardrone_write);
-		fprintf(stderr, "[ardrone_interface] Failed initializing AR.Drone motors, exiting.\n");
+		warnx("motor init fail");
 		thread_running = false;
 		exit(ERROR);
 	}
@@ -378,11 +372,9 @@ int ardrone_interface_thread_main(int argc, char *argv[])
 	int termios_state;
 
 	if ((termios_state = tcsetattr(ardrone_write, TCSANOW, &uart_config_original)) < 0) {
-		fprintf(stderr, "[ardrone_interface] ERROR setting baudrate / termios config for (tcsetattr)\n");
+		warnx("ERR: tcsetattr");
 	}
 
-	printf("[ardrone_interface] Restored original UART config, exiting..\n");
-
 	/* close uarts */
 	close(ardrone_write);
 	ar_multiplexing_deinit(gpios);
diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index fc017dd58..4fa24275f 100644
--- a/src/drivers/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -301,7 +301,7 @@ int ar_init_motors(int ardrone_uart, int gpios)
 	ardrone_write_motor_commands(ardrone_uart, 0, 0, 0, 0);
 
 	if (errcounter != 0) {
-		fprintf(stderr, "[ardrone_interface] init sequence incomplete, failed %d times", -errcounter);
+		warnx("Failed %d times", -errcounter);
 		fflush(stdout);
 	}
 	return errcounter;
-- 
cgit v1.2.3


From 8d5225b9679c2820a57ea30ed6d4c46f5d6c9baa Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 16:41:18 +0100
Subject: FrSky: Be less chatty

---
 src/drivers/frsky_telemetry/frsky_telemetry.c | 9 +++------
 1 file changed, 3 insertions(+), 6 deletions(-)

(limited to 'src')

diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
index 6e0839043..bccdf1190 100644
--- a/src/drivers/frsky_telemetry/frsky_telemetry.c
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -84,7 +84,7 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
 	/* Back up the original UART configuration to restore it after exit */
 	int termios_state;
 	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		warnx("ERROR get termios config %s: %d\n", uart_name, termios_state);
+		warnx("ERR: tcgetattr%s: %d\n", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
@@ -100,13 +100,13 @@ static int frsky_open_uart(const char *uart_name, struct termios *uart_config_or
 	static const speed_t speed = B9600;
 
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
+		warnx("ERR: %s: %d (cfsetispeed, cfsetospeed)\n", uart_name, termios_state);
 		close(uart);
 		return -1;
 	}
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		warnx("ERROR setting baudrate / termios config for %s (tcsetattr)\n", uart_name);
+		warnx("ERR: %s (tcsetattr)\n", uart_name);
 		close(uart);
 		return -1;
 	}
@@ -151,9 +151,6 @@ static int frsky_telemetry_thread_main(int argc, char *argv[])
 		}
 	}
 
-	/* Print welcome text */
-	warnx("FrSky telemetry interface starting...");
-
 	/* Open UART */
 	struct termios uart_config_original;
 	const int uart = frsky_open_uart(device_name, &uart_config_original);
-- 
cgit v1.2.3


From ef27225534df6cca0b5a98b85c6b7cf3364493c0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 16:41:30 +0100
Subject: HIL: Be less chatty

---
 src/drivers/hil/hil.cpp | 2 --
 1 file changed, 2 deletions(-)

(limited to 'src')

diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index f0dc0c651..9b5c8133b 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -442,8 +442,6 @@ HIL::task_main()
 	/* make sure servos are off */
 	// up_pwm_servo_deinit();
 
-	log("stopping");
-
 	/* note - someone else is responsible for restoring the GPIO config */
 
 	/* tell the dtor that we are exiting */
-- 
cgit v1.2.3


From 5af710221a2948ab14f95225b4cb6c7fe5c66560 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 16:54:34 +0100
Subject: HoTT: Less chatty

---
 src/drivers/hott/comms.cpp                         | 8 ++++----
 src/drivers/hott/hott_sensors/hott_sensors.cpp     | 6 +++---
 src/drivers/hott/hott_telemetry/hott_telemetry.cpp | 6 +++---
 3 files changed, 10 insertions(+), 10 deletions(-)

(limited to 'src')

diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp
index cb8bbba37..60a49b559 100644
--- a/src/drivers/hott/comms.cpp
+++ b/src/drivers/hott/comms.cpp
@@ -55,7 +55,7 @@ open_uart(const char *device)
 	const int uart = open(device, O_RDWR | O_NOCTTY);
 
 	if (uart < 0) {
-		err(1, "Error opening port: %s", device);
+		err(1, "ERR: opening %s", device);
 	}
 	
 	/* Back up the original uart configuration to restore it after exit */	
@@ -63,7 +63,7 @@ open_uart(const char *device)
 	struct termios uart_config_original;
 	if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
 		close(uart);
-		err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
+		err(1, "ERR: %s: %d", device, termios_state);
 	}
 
 	/* Fill the struct for the new configuration */
@@ -76,13 +76,13 @@ open_uart(const char *device)
 	/* Set baud rate */
 	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
 		close(uart);
-		err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
+		err(1, "ERR: %s: %d (cfsetispeed, cfsetospeed)",
 			 device, termios_state);
 	}
 
 	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
 		close(uart);
-		err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
+		err(1, "ERR: %s (tcsetattr)", device);
 	}
 
 	/* Activate single wire mode */
diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp
index a3d3a3933..8ab9d8d55 100644
--- a/src/drivers/hott/hott_sensors/hott_sensors.cpp
+++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp
@@ -204,7 +204,7 @@ hott_sensors_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			warnx("deamon already running");
+			warnx("already running");
 			exit(0);
 		}
 
@@ -225,10 +225,10 @@ hott_sensors_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("daemon is running");
+			warnx("is running");
 
 		} else {
-			warnx("daemon not started");
+			warnx("not started");
 		}
 
 		exit(0);
diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
index d293f9954..edbb14172 100644
--- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
+++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
@@ -230,7 +230,7 @@ hott_telemetry_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "start")) {
 
 		if (thread_running) {
-			warnx("deamon already running");
+			warnx("already running");
 			exit(0);
 		}
 
@@ -251,10 +251,10 @@ hott_telemetry_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("daemon is running");
+			warnx("is running");
 
 		} else {
-			warnx("daemon not started");
+			warnx("not started");
 		}
 
 		exit(0);
-- 
cgit v1.2.3


From 1fc7b588945b3bb1df70de1f779201c60a803b71 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 16:54:47 +0100
Subject: Bottle drop: Less chatty

---
 src/modules/bottle_drop/bottle_drop.cpp | 19 +++++++++----------
 1 file changed, 9 insertions(+), 10 deletions(-)

(limited to 'src')

diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index 31c9157e1..6d24e5d2d 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -283,7 +283,6 @@ BottleDrop::drop()
 	// force the door open if we have to
 	if (_doors_opened == 0) {
 		open_bay();
-		warnx("bay not ready, forced open");
 	}
 
 	while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) {
@@ -723,16 +722,16 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
 		if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
 			open_bay();
 			drop();
-			mavlink_log_info(_mavlink_fd, "#audio: drop bottle");
+			mavlink_log_critical(_mavlink_fd, "drop bottle");
 
 		} else if (cmd->param1 > 0.5f) {
 			open_bay();
-			mavlink_log_info(_mavlink_fd, "#audio: opening bay");
+			mavlink_log_critical(_mavlink_fd, "opening bay");
 
 		} else {
 			lock_release();
 			close_bay();
-			mavlink_log_info(_mavlink_fd, "#audio: closing bay");
+			mavlink_log_critical(_mavlink_fd, "closing bay");
 		}
 
 		answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
@@ -743,12 +742,12 @@ BottleDrop::handle_command(struct vehicle_command_s *cmd)
 		switch ((int)(cmd->param1 + 0.5f)) {
 		case 0:
 			_drop_approval = false;
-			mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval");
+			mavlink_log_critical(_mavlink_fd, "got drop position, no approval");
 			break;
 
 		case 1:
 			_drop_approval = true;
-			mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval");
+			mavlink_log_critical(_mavlink_fd, "got drop position and approval");
 			break;
 
 		default:
@@ -818,19 +817,19 @@ BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESUL
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command denied: %u", cmd->command);
 		break;
 
 	case VEHICLE_CMD_RESULT_FAILED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command failed: %u", cmd->command);
 		break;
 
 	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command temporarily rejected: %u", cmd->command);
 		break;
 
 	case VEHICLE_CMD_RESULT_UNSUPPORTED:
-		mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command);
+		mavlink_log_critical(_mavlink_fd, "command unsupported: %u", cmd->command);
 		break;
 
 	default:
-- 
cgit v1.2.3


From beee7a89ed6e7a7c98ee712ab571c58eb6f3a692 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 16:55:04 +0100
Subject: Airspeed: less chatty

---
 src/drivers/meas_airspeed/meas_airspeed.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 1d9a463ad..ba46de379 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -519,7 +519,7 @@ test()
 		ret = poll(&fds, 1, 2000);
 
 		if (ret != 1) {
-			errx(1, "timed out waiting for sensor data");
+			errx(1, "timed out");
 		}
 
 		/* now go get it */
-- 
cgit v1.2.3


From d602c9a0c5eff39e0f533411b45f631cae63e486 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 16:55:18 +0100
Subject: Controllib: Optimize for size

---
 src/modules/controllib/module.mk | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/modules/controllib/module.mk b/src/modules/controllib/module.mk
index f0139a4b7..2860d1efc 100644
--- a/src/modules/controllib/module.mk
+++ b/src/modules/controllib/module.mk
@@ -39,3 +39,5 @@ SRCS		 = test_params.c \
 		   block/BlockParam.cpp \
 		   uorb/blocks.cpp \
 		   blocks.cpp
+
+MAXOPTIMIZATION = -Os
-- 
cgit v1.2.3


From 9c7503ba7a3f5d8f98ab3017d221192489d0b3c9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 18:38:33 +0100
Subject: uORB: Save space, does not do complex operations

---
 src/modules/uORB/module.mk | 2 ++
 1 file changed, 2 insertions(+)

(limited to 'src')

diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk
index 0c29101fe..9385ce253 100644
--- a/src/modules/uORB/module.mk
+++ b/src/modules/uORB/module.mk
@@ -44,3 +44,5 @@ SRCS			= uORB.cpp \
 			  objects_common.cpp \
 			  Publication.cpp \
 			  Subscription.cpp
+
+MAXOPTIMIZATION	 = -Os
-- 
cgit v1.2.3


From 488739cc467cea08374e97f96f70317376ec58ec Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 19:13:46 +0100
Subject: Fix up ver command handling to print MCU version on all command as
 well

---
 src/systemcmds/ver/ver.c | 58 ++++++++++++++++++++++++++++--------------------
 1 file changed, 34 insertions(+), 24 deletions(-)

(limited to 'src')

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 62a7a5b92..eebeb9289 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -46,7 +46,7 @@
 #include <systemlib/err.h>
 #include <systemlib/mcu_version.h>
 
-// string constants for version commands
+/* string constants for version commands */
 static const char sz_ver_hw_str[] 	= "hw";
 static const char sz_ver_hwcmp_str[] = "hwcmp";
 static const char sz_ver_git_str[] 	= "git";
@@ -68,49 +68,56 @@ __EXPORT int ver_main(int argc, char *argv[]);
 
 int ver_main(int argc, char *argv[])
 {
-	int ret = 1;	//defaults to an error
+	/* defaults to an error */
+	int ret = 1;
 
-	// first check if there are at least 2 params
+	/* first check if there are at least 2 params */
 	if (argc >= 2) {
 		if (argv[1] != NULL) {
-			if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
-				printf("%s\n", HW_ARCH);
-				ret = 0;
 
-			} else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
+			if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) {
 				if (argc >= 3 && argv[2] != NULL) {
-					// compare 3rd parameter with HW_ARCH string, in case of match, return 0
+					/* compare 3rd parameter with HW_ARCH string, in case of match, return 0 */
 					ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH));
 
 					if (ret == 0) {
 						printf("ver hwcmp match: %s\n", HW_ARCH);
+						return ret;
 					}
 
 				} else {
 					errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'");
 				}
+			}
 
-			} else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
-				printf("%s\n", FW_GIT);
-				ret = 0;
+			/* check if we want to show all */
+			bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))
 
-			} else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
-				printf("%s %s\n", __DATE__, __TIME__);
+			if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
+				printf("HW arch: %s\n", HW_ARCH);
 				ret = 0;
 
-			} else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
-				printf("%s\n", __VERSION__);
+			}
+
+			if (show_all || !strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) {
+				printf("FW git-hash: %s\n", FW_GIT);
 				ret = 0;
 
-			} else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) {
-				printf("HW arch: %s\n", HW_ARCH);
+			}
+
+			if (show_all || !strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) {
 				printf("Build datetime: %s %s\n", __DATE__, __TIME__);
-				printf("FW git-hash: %s\n", FW_GIT);
-				printf("GCC toolchain: %s\n", __VERSION__);
 				ret = 0;
 
+			}
 
-			} else if (!strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) {
+			if (show_all || !strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) {
+				printf("Toolchain: %s\n", __VERSION__);
+				ret = 0;
+
+			}
+
+			if (show_all || !strncmp(argv[1], mcu_ver_str, sizeof(mcu_ver_str))) {
 
 				char rev;
 				char* revstr;
@@ -118,21 +125,24 @@ int ver_main(int argc, char *argv[])
 				int chip_version = mcu_version(&rev, &revstr);
 
 				if (chip_version < 0) {
-					printf("UNKNOWN MCU");
+					printf("UNKNOWN MCU\n");
 					ret = 1;
 
 				} else {
 					printf("MCU: %s, rev. %c\n", revstr, rev);
 
 					if (chip_version < MCU_REV_STM32F4_REV_3) {
-						printf("\n\nWARNING   WARNING   WARNING!\n"
+						printf("\nWARNING   WARNING   WARNING!\n"
 							"Revision %c has a silicon errata\n"
 							"This device can only utilize a maximum of 1MB flash safely!\n"
-							"http://px4.io/help/errata\n", rev);
+							"http://px4.io/help/errata\n\n", rev);
 					}
 				}
 
-			} else {
+				ret = 0;
+			}
+
+			if (ret = 1) {
 				errx(1, "unknown command.\n");
 			}
 
-- 
cgit v1.2.3


From ed0f28ed4191c296aa3592088dc7875a8246cabb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 19:43:11 +0100
Subject: Fix ver command

---
 src/systemcmds/ver/ver.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index eebeb9289..55cec149f 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -91,7 +91,7 @@ int ver_main(int argc, char *argv[])
 			}
 
 			/* check if we want to show all */
-			bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))
+			bool show_all = !strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str));
 
 			if (show_all || !strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) {
 				printf("HW arch: %s\n", HW_ARCH);
-- 
cgit v1.2.3


From e2551d491754b752b8b835a302acaf3f4cf1f9d6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 20:05:54 +0100
Subject: Fixed typo in ver command

---
 src/systemcmds/ver/ver.c | 3 +--
 1 file changed, 1 insertion(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index 55cec149f..c794f5819 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -126,7 +126,6 @@ int ver_main(int argc, char *argv[])
 
 				if (chip_version < 0) {
 					printf("UNKNOWN MCU\n");
-					ret = 1;
 
 				} else {
 					printf("MCU: %s, rev. %c\n", revstr, rev);
@@ -142,7 +141,7 @@ int ver_main(int argc, char *argv[])
 				ret = 0;
 			}
 
-			if (ret = 1) {
+			if (ret == 1) {
 				errx(1, "unknown command.\n");
 			}
 
-- 
cgit v1.2.3


From 18dc5e342905f6171b54adba6de8a27d65d62ab2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 15 Nov 2014 20:07:32 +0100
Subject: Fix compile warning in MAVLink app

---
 src/modules/mavlink/mavlink_messages.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index b4911427f..bed915bbe 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -382,11 +382,11 @@ protected:
 						clock_gettime(CLOCK_REALTIME, &ts);
 						/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
 						time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9);
-						struct tm t;
-						gmtime_r(&gps_time_sec, &t);
+						struct tm tt;
+						gmtime_r(&gps_time_sec, &tt);
 
 						// XXX we do not want to interfere here with the SD log app
-						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &t);
+						strftime(log_file_name, sizeof(log_file_name), "msgs_%Y_%m_%d_%H_%M_%S.txt", &tt);
 						snprintf(log_file_path, sizeof(log_file_path), "/fs/microsd/%s", log_file_name);
 						fp = fopen(log_file_path, "ab");
 					}
-- 
cgit v1.2.3


From c0f32d44a2acd0f840b4246f90816a95dc6a7527 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 16 Nov 2014 00:15:40 +0100
Subject: First stab at animation

---
 src/modules/px4iofirmware/px4io.c | 40 ++++++++++++++++++++++++++++++++++++++-
 1 file changed, 39 insertions(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 30f32b38e..51281f660 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -125,7 +125,45 @@ heartbeat_blink(void)
 	static bool heartbeat = false;
 	LED_BLUE(heartbeat = !heartbeat);
 #ifdef GPIO_LED4
-	LED_RING(heartbeat);
+
+	const unsigned max_brightness = 1000;
+
+	static unsigned counter = 0;
+	static unsigned brightness = max_brightness;
+	static unsigned brightness_counter = 0;
+	static unsigned on_counter = 0;
+
+	if (brightness_counter < max_brightness) {
+
+		bool on = ((on_counter * 100) / brightness_counter) <= ((brightness * 100) / max_brightness);
+
+		LED_RING(on);
+		brightness_counter++;
+
+		if (on) {
+			on_counter++;
+		}
+
+	} else {
+
+		if (counter >= 62) {
+			counter = 0;
+		}
+
+		int n;
+
+		if (counter < 32) {
+			n = counter;
+
+		} else {
+			n = 62 - counter;
+		}
+
+		brightness = n * n;// designed to be ~1000 / (31.0f * 31.0f);
+		brightness_counter = 0;
+		on_counter = 0;
+	}
+
 #endif
 }
 
-- 
cgit v1.2.3


From 63707ef058f01e324d8d74cf6bc48635e09ab507 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 16 Nov 2014 00:57:23 +0100
Subject: Let it breathe

---
 src/modules/px4iofirmware/px4io.c | 19 ++++++++++++++++---
 1 file changed, 16 insertions(+), 3 deletions(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 51281f660..8abfb0d2e 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -85,6 +85,9 @@ static volatile uint8_t msg_next_out, msg_next_in;
 #define NUM_MSG 2
 static char msg[NUM_MSG][40];
 
+static void heartbeat_blink(void);
+static void ring_blink(void);
+
 /*
  * add a debug message to be printed on the console
  */
@@ -124,9 +127,16 @@ heartbeat_blink(void)
 {
 	static bool heartbeat = false;
 	LED_BLUE(heartbeat = !heartbeat);
+}
+
+static void
+ring_blink(void)
+{
 #ifdef GPIO_LED4
 
-	const unsigned max_brightness = 1000;
+	// XXX this led code does have
+	// intentionally a few magic numbers.
+	const unsigned max_brightness = 118;
 
 	static unsigned counter = 0;
 	static unsigned brightness = max_brightness;
@@ -135,7 +145,7 @@ heartbeat_blink(void)
 
 	if (brightness_counter < max_brightness) {
 
-		bool on = ((on_counter * 100) / brightness_counter) <= ((brightness * 100) / max_brightness);
+		bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1);
 
 		LED_RING(on);
 		brightness_counter++;
@@ -159,9 +169,10 @@ heartbeat_blink(void)
 			n = 62 - counter;
 		}
 
-		brightness = n * n;// designed to be ~1000 / (31.0f * 31.0f);
+		brightness = (n * n) / 9;
 		brightness_counter = 0;
 		on_counter = 0;
+		counter++;
 	}
 
 #endif
@@ -338,6 +349,8 @@ user_start(int argc, char *argv[])
                     heartbeat_blink();
                 }
 
+                ring_blink();
+
                 check_reboot();
 
 		/* check for debug activity (default: none) */
-- 
cgit v1.2.3


From 8583cf4dcc0e88842cdf717de26a4f0daa63729a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 16 Nov 2014 00:59:18 +0100
Subject: Let it breathe stronger

---
 src/modules/px4iofirmware/px4io.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 8abfb0d2e..349aaac57 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -169,7 +169,7 @@ ring_blink(void)
 			n = 62 - counter;
 		}
 
-		brightness = (n * n) / 9;
+		brightness = (n * n) / 8;
 		brightness_counter = 0;
 		on_counter = 0;
 		counter++;
-- 
cgit v1.2.3


From 8caada2188b9c9a8fcab70fbad92ab14506c2427 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 16 Nov 2014 01:21:47 +0100
Subject: Breathe animation, led solid on arming

---
 src/modules/px4iofirmware/px4io.c | 14 ++++++++++++--
 src/modules/px4iofirmware/px4io.h |  2 +-
 2 files changed, 13 insertions(+), 3 deletions(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index 349aaac57..14ee9cb40 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -134,6 +134,12 @@ ring_blink(void)
 {
 #ifdef GPIO_LED4
 
+	if (/* IO armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
+	/* and FMU is armed */ && (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) {
+		LED_RING(1);
+		return;
+	}
+
 	// XXX this led code does have
 	// intentionally a few magic numbers.
 	const unsigned max_brightness = 118;
@@ -147,7 +153,11 @@ ring_blink(void)
 
 		bool on = ((on_counter * 100) / brightness_counter+1) <= ((brightness * 100) / max_brightness+1);
 
-		LED_RING(on);
+		// XXX once led is PWM driven,
+		// remove the ! in the line below
+		// to return to the proper breathe
+		// animation / pattern (currently inverted)
+		LED_RING(!on);
 		brightness_counter++;
 
 		if (on) {
@@ -349,7 +359,7 @@ user_start(int argc, char *argv[])
                     heartbeat_blink();
                 }
 
-                ring_blink();
+		ring_blink();
 
                 check_reboot();
 
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 8186e4c78..93a33490f 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -140,7 +140,7 @@ extern pwm_limit_t pwm_limit;
 #define LED_BLUE(_s)			stm32_gpiowrite(GPIO_LED1, !(_s))
 #define LED_AMBER(_s)			stm32_gpiowrite(GPIO_LED2, !(_s))
 #define LED_SAFETY(_s)			stm32_gpiowrite(GPIO_LED3, !(_s))
-#define LED_RING(_s)			stm32_gpiowrite(GPIO_LED4, !(_s))
+#define LED_RING(_s)			stm32_gpiowrite(GPIO_LED4, (_s))
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 
-- 
cgit v1.2.3


From 4e8387465e01cf16e0fbbe94b5e9a3c168540b77 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 16 Nov 2014 13:38:41 +1000
Subject: navigator: only update sensor_combined topic with 50Hz

---
 src/modules/navigator/navigator_main.cpp | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index e933919fb..ed9e999a9 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -289,8 +289,9 @@ Navigator::task_main()
 	navigation_capabilities_update();
 	params_update();
 
-	/* rate limit position updates to 50 Hz */
+	/* rate limit position and sensor updates to 50 Hz */
 	orb_set_interval(_global_pos_sub, 20);
+	orb_set_interval(_sensor_combined_sub, 20);
 
 	hrt_abstime mavlink_open_time = 0;
 	const hrt_abstime mavlink_open_interval = 500000;
-- 
cgit v1.2.3


From 4d69314a9925be8ded1b9cb723bd01c21f01a816 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 16 Nov 2014 10:55:18 +0100
Subject: Adjust navigator priority

---
 src/modules/navigator/navigator_main.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index ed9e999a9..3ca6ac2c0 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -498,7 +498,7 @@ Navigator::start()
 	/* start the task */
 	_navigator_task = task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 5,
+					 SCHED_PRIORITY_DEFAULT + 20,
 					 2000,
 					 (main_t)&Navigator::task_main_trampoline,
 					 nullptr);
-- 
cgit v1.2.3


From 88bae21ce53fc6acadaae4c8271f3d0fce18a721 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 5 Nov 2014 21:20:32 +1100
Subject: px4io: added RC_CONFIG, SET_OVERRIDE_OK and CLEAR_OVERRIDE_OK ioctls

this allows for full setup of RC override without needing param_get()
to PX4 specific parameters
---
 src/drivers/drv_pwm_output.h | 26 ++++++++++++++++++++++++++
 src/drivers/px4io/px4io.cpp  | 36 ++++++++++++++++++++++++++++++++++++
 2 files changed, 62 insertions(+)

(limited to 'src')

diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 6873f24b6..b41f088eb 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -117,6 +117,23 @@ struct pwm_output_values {
 	unsigned			channel_count;
 };
 
+
+/**
+ * RC config values for a channel
+ *
+ * This allows for PX4IO_PAGE_RC_CONFIG values to be set without a
+ * param_get() dependency
+ */
+struct pwm_output_rc_config {
+	uint8_t channel;
+	uint16_t rc_min;
+	uint16_t rc_trim;
+	uint16_t rc_max;
+	uint16_t rc_dz;
+	uint16_t rc_assignment;
+	bool     rc_reverse;
+};
+
 /*
  * ORB tag for PWM outputs.
  */
@@ -216,6 +233,15 @@ ORB_DECLARE(output_pwm);
 /** force safety switch on (to enable use of safety switch) */
 #define PWM_SERVO_SET_FORCE_SAFETY_ON  _IOC(_PWM_SERVO_BASE, 26)
 
+/** set RC config for a channel. This takes a pointer to pwm_output_rc_config */
+#define PWM_SERVO_SET_RC_CONFIG  _IOC(_PWM_SERVO_BASE, 27)
+
+/** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
+#define PWM_SERVO_SET_OVERRIDE_OK	_IOC(_PWM_SERVO_BASE, 28)
+
+/** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
+#define PWM_SERVO_CLEAR_OVERRIDE_OK	_IOC(_PWM_SERVO_BASE, 29)
+
 /*
  *
  *
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 06faf49a4..dc33f7657 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -2572,6 +2572,42 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 
 		break;
 
+	case PWM_SERVO_SET_RC_CONFIG: {
+		/* enable setting of RC configuration without relying
+		   on param_get() 
+		*/
+		struct pwm_output_rc_config* config = (struct pwm_output_rc_config*)arg;
+		if (config->channel >= _max_actuators) {
+			/* fail with error */
+			return E2BIG;
+		}
+
+		/* copy values to registers in IO */
+		uint16_t regs[PX4IO_P_RC_CONFIG_STRIDE];
+		uint16_t offset = config->channel * PX4IO_P_RC_CONFIG_STRIDE;
+		regs[PX4IO_P_RC_CONFIG_MIN]        = config->rc_min;
+		regs[PX4IO_P_RC_CONFIG_CENTER]     = config->rc_trim;
+		regs[PX4IO_P_RC_CONFIG_MAX]        = config->rc_max;
+		regs[PX4IO_P_RC_CONFIG_DEADZONE]   = config->rc_dz;
+		regs[PX4IO_P_RC_CONFIG_ASSIGNMENT] = config->rc_assignment;
+		regs[PX4IO_P_RC_CONFIG_OPTIONS]    = PX4IO_P_RC_CONFIG_OPTIONS_ENABLED;
+		if (config->rc_reverse) {
+			regs[PX4IO_P_RC_CONFIG_OPTIONS] |= PX4IO_P_RC_CONFIG_OPTIONS_REVERSE;
+		}
+		ret = io_reg_set(PX4IO_PAGE_RC_CONFIG, offset, regs, PX4IO_P_RC_CONFIG_STRIDE);
+		break;
+	}
+
+	case PWM_SERVO_SET_OVERRIDE_OK:
+		/* set the 'OVERRIDE OK' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK);
+		break;
+
+	case PWM_SERVO_CLEAR_OVERRIDE_OK:
+		/* clear the 'OVERRIDE OK' bit */
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0);
+		break;
+
 	default:
 		/* see if the parent class can make any use of it */
 		ret = CDev::ioctl(filep, cmd, arg);
-- 
cgit v1.2.3


From 6406e235d6f5cb37a914442036111f616b4b3839 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Thu, 6 Nov 2014 10:32:21 +1100
Subject: px4io: publish pwm values when STATUS_FLAGS_FMU_OK is not set

this allows testing of FMU failure behaviour in px4io by monitoring
the reported PWM output when the vehicle code stops sending
updates. Otherwise testing needs to be done with "px4io status" which
is very tedious.

With this change a GCS can monitor the PWM outputs from the failsafe
mixer using normal mavlink messages
---
 src/drivers/px4io/px4io.cpp | 4 ----
 1 file changed, 4 deletions(-)

(limited to 'src')

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index dc33f7657..6a313b322 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1657,10 +1657,6 @@ PX4IO::io_publish_raw_rc()
 int
 PX4IO::io_publish_pwm_outputs()
 {
-	/* if no FMU comms(!) just don't publish */
-	if (!(_status & PX4IO_P_STATUS_FLAGS_FMU_OK))
-		return OK;
-
 	/* data we are going to fetch */
 	actuator_outputs_s outputs;
 	outputs.timestamp = hrt_absolute_time();
-- 
cgit v1.2.3


From ba811254536b6a080d4b3379106bf0e648a51eb0 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 5 Nov 2014 21:20:03 +1100
Subject: px4io: added OVERRIDE_IMMEDIATE arming flag

this allows the flight code to choose whether FMU failure gives
immediate manual pilot control, or waits for the mode switch to go
past the override threshold
---
 src/drivers/drv_pwm_output.h          |  3 +++
 src/drivers/px4io/px4io.cpp           | 18 ++++++++++++++++--
 src/modules/px4iofirmware/controls.c  |  9 +++++++++
 src/modules/px4iofirmware/protocol.h  |  1 +
 src/modules/px4iofirmware/registers.c |  3 ++-
 5 files changed, 31 insertions(+), 3 deletions(-)

(limited to 'src')

diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index b41f088eb..edb72f04e 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -242,6 +242,9 @@ ORB_DECLARE(output_pwm);
 /** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
 #define PWM_SERVO_CLEAR_OVERRIDE_OK	_IOC(_PWM_SERVO_BASE, 29)
 
+/** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
+#define PWM_SERVO_SET_OVERRIDE_IMMEDIATE	_IOC(_PWM_SERVO_BASE, 30)
+
 /*
  *
  *
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 6a313b322..519ba663a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -2057,7 +2057,7 @@ PX4IO::print_status(bool extended_status)
 		((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
 		);
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
-	printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
+	printf("arming 0x%04x%s%s%s%s%s%s%s%s%s%s\n",
 	       arming,
 	       ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)		? " FMU_ARMED" : " FMU_DISARMED"),
 	       ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)		? " IO_ARM_OK" : " IO_ARM_DENIED"),
@@ -2067,7 +2067,8 @@ PX4IO::print_status(bool extended_status)
 	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)	? " ALWAYS_PWM_ENABLE" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)		? " LOCKDOWN" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)		? " FORCE_FAILSAFE" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : "")
+	       ((arming & PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE) ? " TERM_FAILSAFE" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) ? " OVERRIDE_IMMEDIATE" : "")
 	       );
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
@@ -2307,6 +2308,19 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		}
 		break;
 
+	case PWM_SERVO_SET_OVERRIDE_IMMEDIATE:
+		/* control whether override on FMU failure is
+                   immediate or waits for override threshold on mode
+                   switch */
+		if (arg == 0) {
+			/* clear override immediate flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE, 0);
+		} else {
+			/* set override immediate flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE);
+		}
+		break;
+
 	case DSM_BIND_START:
 
 		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index ad60ee03e..e3cb8d820 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -417,6 +417,15 @@ controls_tick() {
 		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
 			override = true;
 
+		/*
+		  if the FMU is dead then enable override if we have a
+		  mixer and OVERRIDE_IMMEDIATE is set
+		 */
+		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
+		    (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
+		    (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
+			override = true;                
+
 		if (override) {
 
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 9b2e047cb..c7e9ae3eb 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -183,6 +183,7 @@
 #define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 7) /* If set, the system operates normally, but won't actuate any servos */
 #define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE	(1 << 8) /* If set, the system will always output the failsafe values */
 #define PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE	(1 << 9) /* If set, the system will never return from a failsafe, but remain in failsafe once triggered. */
+#define PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE	(1 << 10) /* If set then on FMU failure override is immediate. Othewise it waits for the mode switch to go past the override thrshold */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 49c2a9f56..fbfdd35db 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -191,7 +191,8 @@ volatile uint16_t	r_page_setup[] =
 					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
 					 PX4IO_P_SETUP_ARMING_LOCKDOWN | \
 					 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE | \
-					 PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE)
+					 PX4IO_P_SETUP_ARMING_TERMINATION_FAILSAFE | \
+					 PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
-- 
cgit v1.2.3


From f533c362515e31b536c34d83342c396e54a7d652 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Wed, 5 Nov 2014 21:20:32 +1100
Subject: px4io: added RC_CONFIG, SET_OVERRIDE_OK and CLEAR_OVERRIDE_OK ioctls

this allows for full setup of RC override without needing param_get()
to PX4 specific parameters

Conflicts:
	src/drivers/drv_pwm_output.h
---
 src/drivers/drv_pwm_output.h | 8 ++++----
 1 file changed, 4 insertions(+), 4 deletions(-)

(limited to 'src')

diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index edb72f04e..b10c3e18a 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -231,16 +231,16 @@ ORB_DECLARE(output_pwm);
 #define PWM_SERVO_SET_TERMINATION_FAILSAFE	_IOC(_PWM_SERVO_BASE, 25)
 
 /** force safety switch on (to enable use of safety switch) */
-#define PWM_SERVO_SET_FORCE_SAFETY_ON  _IOC(_PWM_SERVO_BASE, 26)
+#define PWM_SERVO_SET_FORCE_SAFETY_ON		_IOC(_PWM_SERVO_BASE, 26)
 
 /** set RC config for a channel. This takes a pointer to pwm_output_rc_config */
-#define PWM_SERVO_SET_RC_CONFIG  _IOC(_PWM_SERVO_BASE, 27)
+#define PWM_SERVO_SET_RC_CONFIG			_IOC(_PWM_SERVO_BASE, 27)
 
 /** set the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
-#define PWM_SERVO_SET_OVERRIDE_OK	_IOC(_PWM_SERVO_BASE, 28)
+#define PWM_SERVO_SET_OVERRIDE_OK		_IOC(_PWM_SERVO_BASE, 28)
 
 /** clear the 'OVERRIDE OK' bit, which allows for RC control on FMU loss */
-#define PWM_SERVO_CLEAR_OVERRIDE_OK	_IOC(_PWM_SERVO_BASE, 29)
+#define PWM_SERVO_CLEAR_OVERRIDE_OK		_IOC(_PWM_SERVO_BASE, 29)
 
 /** setup OVERRIDE_IMMEDIATE behaviour on FMU fail */
 #define PWM_SERVO_SET_OVERRIDE_IMMEDIATE	_IOC(_PWM_SERVO_BASE, 30)
-- 
cgit v1.2.3


From 852fa611730be2433156b010c6162606ec857ef3 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Sun, 16 Nov 2014 14:12:58 -0800
Subject: Correct setting of DSM and ST24 flags

---
 src/modules/px4iofirmware/controls.c | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 3fd73fc60..75d80ba5d 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -71,7 +71,6 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated)
 	uint8_t *bytes;
 	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
 	if (*dsm_updated) {
-		r_raw_rc_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
 		r_raw_rc_count = temp_count & 0x7fff;
 		if (temp_count & 0x8000)
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
@@ -172,6 +171,12 @@ controls_tick() {
 	perf_begin(c_gather_dsm);
 	bool dsm_updated, st24_updated;
 	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated);
+	if (dsm_updated) {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+	}
+	if (st24_updated) {
+		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
+	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
-- 
cgit v1.2.3


From 52d5a7c00aa74fe0d4241f7e6636dfd6e48d0acf Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 16 Nov 2014 23:24:52 +0100
Subject: Fix PWM command

---
 src/systemcmds/pwm/pwm.c | 68 ++++++++++++++++++++++++------------------------
 1 file changed, 34 insertions(+), 34 deletions(-)

(limited to 'src')

diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index 478c2a772..eeba89fa8 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -74,34 +74,34 @@ usage(const char *reason)
 		"usage:\n"
 		"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info  ...\n"
 		"\n"
-		"  arm                      Arm output\n"
-		"  disarm                   Disarm output\n"
+		"arm\t\t\t\tArm output\n"
+		"disarm\t\t\t\tDisarm output\n"
 		"\n"
-		"  rate ...                 Configure PWM rates\n"
-		"    [-g <channel group>]   Channel group that should update at the alternate rate\n"
-		"    [-m <chanmask> ]       Directly supply channel mask\n"
-		"    [-a]                   Configure all outputs\n"
-		"    -r <alt_rate>          PWM rate (50 to 400 Hz)\n"
+		"rate ...\t\t\tConfigure PWM rates\n"
+		"\t[-g <channel group>]\t(e.g. 0,1,2)\n"
+		"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
+		"\t[-a]\t\t\tConfigure all outputs\n"
+		"\t-r <alt_rate>\t\tPWM rate (50 to 400 Hz)\n"
 		"\n"
-		"  failsafe ...      	    Configure failsafe PWM values\n"
-		"  disarmed ...      	    Configure disarmed PWM values\n"
-		"  min ...           	    Configure minimum PWM values\n"
-		"  max ...           	    Configure maximum PWM values\n"
-		"    [-c <channels>]        Supply channels (e.g. 1234)\n"
-		"    [-m <chanmask> ]       Directly supply channel mask (e.g. 0xF)\n"
-		"    [-a]                   Configure all outputs\n"
-		"    -p <pwm value>         PWM value\n"
+		"failsafe ...\t\t\tFailsafe PWM\n"
+		"disarmed ...\t\t\tDisarmed PWM\n"
+		"min ...\t\t\t\tMinimum PWM\n"
+		"max ...\t\t\t\tMaximum PWM\n"
+		"\t[-c <channels>]\t\t(e.g. 1234)\n"
+		"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
+		"\t[-a]\t\t\tConfigure all outputs\n"
+		"\t-p <pwm value>\t\tPWM value\n"
 		"\n"
-		"  test ...                 Directly set PWM values\n"
-		"    [-c <channels>]        Supply channels (e.g. 1234)\n"
-		"    [-m <chanmask> ]       Directly supply channel mask (e.g. 0xF)\n"
-		"    [-a]                   Configure all outputs\n"
-		"    -p <pwm value>         PWM value\n"
+		"test ...\t\t\tDirectly set PWM\n"
+		"\t[-c <channels>]\t\t(e.g. 1234)\n"
+		"\t[-m <channel mask> ]\t(e.g. 0xF)\n"
+		"\t[-a]\t\t\tConfigure all outputs\n"
+		"\t-p <pwm value>\t\tPWM value\n"
 		"\n"
-		"  info                     Print information about the PWM device\n"
+		"info\t\t\t\tPrint information\n"
 		"\n"
-		"    -v                     Print verbose information\n"
-		"    -d <device>            PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+		"\t-v\t\t\tVerbose\n"
+		"\t-d <dev>\t\t(default " PWM_OUTPUT_DEVICE_PATH ")\n"
 		);
 
 }
@@ -123,7 +123,7 @@ pwm_main(int argc, char *argv[])
 	unsigned single_ch = 0;
 	unsigned pwm_value = 0;
 
-	if (argc < 1)
+	if (argc < 2)
 		usage(NULL);
 
 	while ((ch = getopt(argc-1, &argv[1], "d:vc:g:m:ap:r:")) != EOF) {
@@ -165,7 +165,7 @@ pwm_main(int argc, char *argv[])
 			/* Read in mask directly */
 			set_mask = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad set_mask value");
+				usage("BAD set_mask VAL");
 			break;
 
 		case 'a':
@@ -176,12 +176,12 @@ pwm_main(int argc, char *argv[])
 		case 'p':
 			pwm_value = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad PWM value provided");
+				usage("BAD PWM VAL");
 			break;
 		case 'r':
 			alt_rate = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad alternative rate provided");
+				usage("BAD rate VAL");
 			break;
 		default:
 			break;
@@ -189,7 +189,7 @@ pwm_main(int argc, char *argv[])
 	}
 
 	if (print_verbose && set_mask > 0) {
-		warnx("Chose channels: ");
+		warnx("Channels: ");
 		printf("    ");
 		for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
 			if (set_mask & 1<<i)
@@ -364,7 +364,7 @@ pwm_main(int argc, char *argv[])
 			usage("no channels set");
 		}
 		if (pwm_value == 0)
-			usage("no PWM value provided");
+			usage("no PWM provided");
 
 		struct pwm_output_values pwm_values = {.values = {0}, .channel_count = 0};
 
@@ -383,7 +383,7 @@ pwm_main(int argc, char *argv[])
 
 			ret = ioctl(fd, PWM_SERVO_SET_FAILSAFE_PWM, (long unsigned int)&pwm_values);
 			if (ret != OK)
-				errx(ret, "failed setting failsafe values");
+				errx(ret, "BAD input VAL");
 		}
 		exit(0);
 
@@ -393,7 +393,7 @@ pwm_main(int argc, char *argv[])
 			usage("no channels set");
 		}
 		if (pwm_value == 0)
-			usage("no PWM value provided");
+			usage("no PWM provided");
 
 		/* get current servo values */
 		struct pwm_output_values last_spos;
@@ -487,7 +487,7 @@ pwm_main(int argc, char *argv[])
 			steps_timing_index < sizeof(steps_timings_us) / sizeof(steps_timings_us[0]);
 			steps_timing_index++ ) {
 
-			warnx("Sending step input with 0 to 100%% over a %u microseconds ramp", steps_timings_us[steps_timing_index]);
+			warnx("Step input (0 to 100%%) over %u us ramp", steps_timings_us[steps_timing_index]);
 
 			while (1) {
 				for (unsigned i = 0; i < servo_count; i++) {
@@ -526,7 +526,7 @@ pwm_main(int argc, char *argv[])
 												err(1, "PWM_SERVO_SET(%d)", i);
 										}
 									}
-						warnx("Key pressed, user abort\n");
+						warnx("User abort\n");
 						exit(0);
 					}
 				}
@@ -675,7 +675,7 @@ pwm_main(int argc, char *argv[])
 		exit(0);
 	}
 
-	usage("specify arm|disarm|rate|failsafe\n\t\tdisarmed|min|max|test|info|forcefail|terminatefail");
+	usage(NULL);
 	return 0;
 }
 
-- 
cgit v1.2.3


From 9d7a4a59fd37cb0dd2fa5534786f87a933e9f6c0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 17 Nov 2014 14:07:13 +0100
Subject: Fix output of ver hwcmd call

---
 src/systemcmds/ver/ver.c | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c
index c794f5819..2ead3e632 100644
--- a/src/systemcmds/ver/ver.c
+++ b/src/systemcmds/ver/ver.c
@@ -82,8 +82,8 @@ int ver_main(int argc, char *argv[])
 
 					if (ret == 0) {
 						printf("ver hwcmp match: %s\n", HW_ARCH);
-						return ret;
 					}
+					return ret;
 
 				} else {
 					errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'");
-- 
cgit v1.2.3


From f20f85f0e3c61ce754fd6d9d73745eaa925b3b04 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 17 Nov 2014 14:07:33 +0100
Subject: Do not spam filter resets in static mode

---
 src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 685f5e12f..00900c995 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -613,8 +613,11 @@ FixedwingEstimator::check_filter_state()
 			warn_index = max_warn_index;
 		}
 
-		warnx("reset: %s", feedback[warn_index]);
-		mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
+		// Do not warn about accel offset if we have no position updates
+		if (!(warn_index == 5 && _ekf->staticMode)) {
+			warnx("reset: %s", feedback[warn_index]);
+			mavlink_log_critical(_mavlink_fd, "[ekf check] %s", feedback[warn_index]);
+		}
 	}
 
 	struct estimator_status_report rep;
-- 
cgit v1.2.3


From 41fe04776f362cf9f0fae6451550df8d5eaf8d99 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 17 Nov 2014 14:59:36 +0100
Subject: Fix up stack sizes

---
 src/modules/bottle_drop/bottle_drop.cpp                          | 2 +-
 src/modules/bottle_drop/module.mk                                | 2 ++
 src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 2 +-
 src/modules/mavlink/mavlink_main.cpp                             | 2 +-
 src/modules/navigator/navigator_main.cpp                         | 2 +-
 5 files changed, 6 insertions(+), 4 deletions(-)

(limited to 'src')

diff --git a/src/modules/bottle_drop/bottle_drop.cpp b/src/modules/bottle_drop/bottle_drop.cpp
index 6d24e5d2d..e0bcbc6e9 100644
--- a/src/modules/bottle_drop/bottle_drop.cpp
+++ b/src/modules/bottle_drop/bottle_drop.cpp
@@ -223,7 +223,7 @@ BottleDrop::start()
 	_main_task = task_spawn_cmd("bottle_drop",
 				    SCHED_DEFAULT,
 				    SCHED_PRIORITY_DEFAULT + 15,
-				    2048,
+				    1500,
 				    (main_t)&BottleDrop::task_main_trampoline,
 				    nullptr);
 
diff --git a/src/modules/bottle_drop/module.mk b/src/modules/bottle_drop/module.mk
index 960228879..df9f5473b 100644
--- a/src/modules/bottle_drop/module.mk
+++ b/src/modules/bottle_drop/module.mk
@@ -41,3 +41,5 @@ SRCS			= bottle_drop.cpp \
 			  bottle_drop_params.c
 
 MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 1200
diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 00900c995..e7805daa9 100644
--- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -1560,7 +1560,7 @@ FixedwingEstimator::start()
 	_estimator_task = task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 40,
-					 5000,
+					 7500,
 					 (main_t)&FixedwingEstimator::task_main_trampoline,
 					 nullptr);
 
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index c251dd3b2..fb9f65cf5 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -1636,7 +1636,7 @@ Mavlink::start(int argc, char *argv[])
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
-		       2900,
+		       2800,
 		       (main_t)&Mavlink::start_helper,
 		       (const char **)argv);
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 3ca6ac2c0..10a4ee88f 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -499,7 +499,7 @@ Navigator::start()
 	_navigator_task = task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT + 20,
-					 2000,
+					 1800,
 					 (main_t)&Navigator::task_main_trampoline,
 					 nullptr);
 
-- 
cgit v1.2.3


From 5f6d03099e07e20abe9cec685fee1244d7c9a0d4 Mon Sep 17 00:00:00 2001
From: Steven Blass <steven.r.blass@gmail.com>
Date: Mon, 17 Nov 2014 19:58:18 -0500
Subject: fixed yaw/yawrate bit masking. fixed navigator overriding offboard
 setpoint

---
 src/modules/mavlink/mavlink_receiver.cpp | 4 ++--
 src/modules/navigator/navigator_main.cpp | 7 ++++---
 2 files changed, 6 insertions(+), 5 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index bc092c7e9..c0a8d7322 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -540,10 +540,10 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 		}
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
 			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 10)) <<
-					OFB_IGN_BIT_YAW;
+					(OFB_IGN_BIT_YAW - 10);
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
 			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 11)) <<
-					OFB_IGN_BIT_YAWRATE;
+					(OFB_IGN_BIT_YAWRATE - 11);
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 10a4ee88f..5e9806499 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -468,9 +468,10 @@ Navigator::task_main()
 			_navigation_mode_array[i]->run(_navigation_mode == _navigation_mode_array[i]);
 		}
 
-		/* if nothing is running, set position setpoint triplet invalid */
-		if (_navigation_mode == nullptr) {
-			// TODO publish empty sp only once
+		/* if nothing is running, set position setpoint triplet invalid once */
+		static bool published_once = false;
+		if (_navigation_mode == nullptr && !published_once) {
+			published_once = true;
 			_pos_sp_triplet.previous.valid = false;
 			_pos_sp_triplet.current.valid = false;
 			_pos_sp_triplet.next.valid = false;
-- 
cgit v1.2.3


From 4eb930f704018253a4cff3604cb9faee8468283b Mon Sep 17 00:00:00 2001
From: Steven Blass <steven.r.blass@gmail.com>
Date: Tue, 18 Nov 2014 08:26:29 -0500
Subject: improved readability of offboard fixes

---
 src/modules/mavlink/mavlink_receiver.cpp | 12 ++++++++----
 src/modules/navigator/navigator.h        |  1 +
 src/modules/navigator/navigator_main.cpp |  6 +++---
 3 files changed, 12 insertions(+), 7 deletions(-)

(limited to 'src')

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c0a8d7322..ca00d1a67 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -538,12 +538,16 @@ MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t
 			offboard_control_sp.ignore &=  ~(1 << i);
 			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
 		}
+
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 10)) <<
-					(OFB_IGN_BIT_YAW - 10);
+		if (set_position_target_local_ned.type_mask & (1 << 10)) {
+			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAW);
+		}
+
 		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
-			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 11)) <<
-					(OFB_IGN_BIT_YAWRATE - 11);
+		if (set_position_target_local_ned.type_mask & (1 << 11)) {
+			offboard_control_sp.ignore |=  (1 << OFB_IGN_BIT_YAWRATE);
+		}
 
 		offboard_control_sp.timestamp = hrt_absolute_time();
 
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index d550dcc4c..9cd609955 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -205,6 +205,7 @@ private:
 
 	bool		_can_loiter_at_sp;			/**< flags if current position SP can be used to loiter */
 	bool		_pos_sp_triplet_updated;		/**< flags if position SP triplet needs to be published */
+	bool 		_pos_sp_triplet_published_invalid_once;	/**< flags if position SP triplet has been published once to UORB */
 
 	control::BlockParamFloat _param_loiter_radius;	/**< loiter radius for fixedwing */
 	control::BlockParamFloat _param_acceptance_radius;	/**< acceptance for takeoff */
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 5e9806499..035b0d18c 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -137,6 +137,7 @@ Navigator::Navigator() :
 	_gpsFailure(this, "GPSF"),
 	_can_loiter_at_sp(false),
 	_pos_sp_triplet_updated(false),
+	_pos_sp_triplet_published_invalid_once(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD"),
 	_param_datalinkloss_obc(this, "DLL_OBC"),
@@ -469,9 +470,8 @@ Navigator::task_main()
 		}
 
 		/* if nothing is running, set position setpoint triplet invalid once */
-		static bool published_once = false;
-		if (_navigation_mode == nullptr && !published_once) {
-			published_once = true;
+		if (_navigation_mode == nullptr && !_pos_sp_triplet_published_invalid_once) {
+			_pos_sp_triplet_published_invalid_once = true;
 			_pos_sp_triplet.previous.valid = false;
 			_pos_sp_triplet.current.valid = false;
 			_pos_sp_triplet.next.valid = false;
-- 
cgit v1.2.3


From 69271a7251a0bcfd980c3534194b73fc45fa5f23 Mon Sep 17 00:00:00 2001
From: Steven Blass <steven.r.blass@gmail.com>
Date: Tue, 18 Nov 2014 08:40:52 -0500
Subject: made invalid setpoints publish once every time it enters an invalid
 state

---
 src/modules/navigator/navigator_main.cpp | 9 ++++++++-
 1 file changed, 8 insertions(+), 1 deletion(-)

(limited to 'src')

diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 035b0d18c..df620e5e7 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -428,12 +428,15 @@ Navigator::task_main()
 				_can_loiter_at_sp = false;
 				break;
 			case NAVIGATION_STATE_AUTO_MISSION:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_mission;
 				break;
 			case NAVIGATION_STATE_AUTO_LOITER:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_loiter;
 				break;
 			case NAVIGATION_STATE_AUTO_RCRECOVER:
+				_pos_sp_triplet_published_invalid_once = false;
 				if (_param_rcloss_obc.get() != 0) {
 					_navigation_mode = &_rcLoss;
 				} else {
@@ -441,11 +444,13 @@ Navigator::task_main()
 				}
 				break;
 			case NAVIGATION_STATE_AUTO_RTL:
-					_navigation_mode = &_rtl;
+				_pos_sp_triplet_published_invalid_once = false;
+				_navigation_mode = &_rtl;
 				break;
 			case NAVIGATION_STATE_AUTO_RTGS:
 				/* Use complex data link loss mode only when enabled via param
 				* otherwise use rtl */
+				_pos_sp_triplet_published_invalid_once = false;
 				if (_param_datalinkloss_obc.get() != 0) {
 					_navigation_mode = &_dataLinkLoss;
 				} else {
@@ -453,9 +458,11 @@ Navigator::task_main()
 				}
 				break;
 			case NAVIGATION_STATE_AUTO_LANDENGFAIL:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_engineFailure;
 				break;
 			case NAVIGATION_STATE_AUTO_LANDGPSFAIL:
+				_pos_sp_triplet_published_invalid_once = false;
 				_navigation_mode = &_gpsFailure;
 				break;
 			default:
-- 
cgit v1.2.3


From 2ce2d26d52884f868ecbcb102f38cbc853c2dbf6 Mon Sep 17 00:00:00 2001
From: Holger Steinhaus <holger@steinhaus-home.de>
Date: Tue, 18 Nov 2014 14:58:04 +0100
Subject: UAVCAN: preserve original UAVCAN message timestamps

---
 src/modules/uavcan/sensors/baro.cpp | 6 +-----
 src/modules/uavcan/sensors/gnss.cpp | 2 +-
 src/modules/uavcan/sensors/gnss.hpp | 2 --
 src/modules/uavcan/sensors/mag.cpp  | 3 +--
 4 files changed, 3 insertions(+), 10 deletions(-)

(limited to 'src')

diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp
index 80c5e3828..8741ae20d 100644
--- a/src/modules/uavcan/sensors/baro.cpp
+++ b/src/modules/uavcan/sensors/baro.cpp
@@ -91,11 +91,7 @@ void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<
 {
 	auto report = ::baro_report();
 
-	report.timestamp = msg.getUtcTimestamp().toUSec();
-	if (report.timestamp == 0) {
-		report.timestamp = msg.getMonotonicTimestamp().toUSec();
-	}
-
+	report.timestamp   = msg.getMonotonicTimestamp().toUSec();
 	report.temperature = msg.static_temperature;
 	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
 
diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp
index 24afe6aaf..a375db37f 100644
--- a/src/modules/uavcan/sensors/gnss.cpp
+++ b/src/modules/uavcan/sensors/gnss.cpp
@@ -92,7 +92,7 @@ void UavcanGnssBridge::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavca
 
 	auto report = ::vehicle_gps_position_s();
 
-	report.timestamp_position = hrt_absolute_time();
+	report.timestamp_position = msg.getMonotonicTimestamp().toUSec();
 	report.lat = msg.latitude_deg_1e8 / 10;
 	report.lon = msg.longitude_deg_1e8 / 10;
 	report.alt = msg.height_msl_mm;
diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp
index e8466b401..2111ff80b 100644
--- a/src/modules/uavcan/sensors/gnss.hpp
+++ b/src/modules/uavcan/sensors/gnss.hpp
@@ -43,8 +43,6 @@
 
 #pragma once
 
-#include <drivers/drv_hrt.h>
-
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_gps_position.h>
 
diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp
index 5cbb96433..35ebee542 100644
--- a/src/modules/uavcan/sensors/mag.cpp
+++ b/src/modules/uavcan/sensors/mag.cpp
@@ -37,7 +37,6 @@
 
 #include "mag.hpp"
 
-#include <drivers/drv_hrt.h>
 #include <systemlib/err.h>
 
 static const orb_id_t MAG_TOPICS[3] = {
@@ -140,7 +139,7 @@ void UavcanMagnetometerBridge::mag_sub_cb(const uavcan::ReceivedDataStructure<ua
 {
 	lock();
 	_report.range_ga = 1.3F;   // Arbitrary number, doesn't really mean anything
-	_report.timestamp = hrt_absolute_time();
+	_report.timestamp = msg.getMonotonicTimestamp().toUSec();
 
 	_report.x = (msg.magnetic_field[0] - _scale.x_offset) * _scale.x_scale;
 	_report.y = (msg.magnetic_field[1] - _scale.y_offset) * _scale.y_scale;
-- 
cgit v1.2.3


From a36088b9c20e52e37d48e7a08aca39d5f8b901f5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 22 Nov 2014 16:29:08 +0100
Subject: INAV: use int for outputs

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp            |  4 ++--
 .../position_estimator_inav/position_estimator_inav_main.c    | 11 ++++++-----
 2 files changed, 8 insertions(+), 7 deletions(-)

(limited to 'src')

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index d52138522..769f84cb6 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -535,7 +535,7 @@ MulticopterPositionControl::reset_pos_sp()
 				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
 		_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
 				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
-		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
+		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %d, %d", (int)_pos_sp(0), (int)_pos_sp(1));
 	}
 }
 
@@ -545,7 +545,7 @@ MulticopterPositionControl::reset_alt_sp()
 	if (_reset_alt_sp) {
 		_reset_alt_sp = false;
 		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
-		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
+		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %d", -(int)_pos_sp(2));
 	}
 }
 
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 81bbaa658..c46d22733 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -389,8 +389,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					} else {
 						wait_baro = false;
 						baro_offset /= (float) baro_init_cnt;
-						warnx("baro offs: %.2f", (double)baro_offset);
-						mavlink_log_info(mavlink_fd, "[inav] baro offs: %.2f", (double)baro_offset);
+						warnx("baro offs: %d", (int)baro_offset);
+						mavlink_log_info(mavlink_fd, "[inav] baro offs: %d", (int)baro_offset);
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
 					}
@@ -520,7 +520,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							sonar_valid_time = t;
 							sonar_valid = true;
 							local_pos.surface_bottom_timestamp = t;
-							mavlink_log_info(mavlink_fd, "[inav] new surface level: %.2f", (double)surface_offset);
+							mavlink_log_info(mavlink_fd, "[inav] new surface level: %d", (int)surface_offset);
 						}
 
 					} else {
@@ -721,8 +721,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 							/* initialize projection */
 							map_projection_init(&ref, lat, lon);
-							warnx("init ref: lat=%.7f, lon=%.7f, alt=%.2f", (double)lat, (double)lon, (double)alt);
-							mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %.2f", (double)lat, (double)lon, (double)alt);
+							// XXX replace this print
+							warnx("init ref: lat=%.7f, lon=%.7f, alt=%8.4f", (double)lat, (double)lon, (double)alt);
+							mavlink_log_info(mavlink_fd, "[inav] init ref: %.7f, %.7f, %8.4f", (double)lat, (double)lon, (double)alt);
 						}
 					}
 
-- 
cgit v1.2.3


From 3c5c1d3c89639d3e67086386d23d10e0dc8f65b0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 22 Nov 2014 15:08:54 +0100
Subject: Fix FD for commander arm operation

---
 src/modules/commander/commander.cpp | 8 ++++++--
 1 file changed, 6 insertions(+), 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index ec173c12b..e081955d0 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -310,12 +310,16 @@ int commander_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "arm")) {
-		arm_disarm(true, mavlink_fd, "command line");
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		arm_disarm(true, mavlink_fd_local, "command line");
+		close(mavlink_fd_local);
 		exit(0);
 	}
 
 	if (!strcmp(argv[1], "disarm")) {
-		arm_disarm(false, mavlink_fd, "command line");
+		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
+		arm_disarm(false, mavlink_fd_local, "command line");
+		close(mavlink_fd_local);
 		exit(0);
 	}
 
-- 
cgit v1.2.3


From 54e7ed70e135f20a3470ebc71dae700e8c223102 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 22 Nov 2014 15:46:28 +0100
Subject: GPS: be less verbose

---
 src/drivers/gps/gps.cpp | 1 -
 1 file changed, 1 deletion(-)

(limited to 'src')

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 5fb4b9ff8..47c907bd3 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -274,7 +274,6 @@ GPS::task_main_trampoline(void *arg)
 void
 GPS::task_main()
 {
-	log("starting");
 
 	/* open the serial port */
 	_serial_fd = ::open(_port, O_RDWR);
-- 
cgit v1.2.3


From b34b40622ba7bbd08e7ca5bc0571612ba96fc7ad Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 22 Nov 2014 15:46:51 +0100
Subject: EKF: less verbose

---
 src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp | 4 ----
 1 file changed, 4 deletions(-)

(limited to 'src')

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 35dc39ec6..e2dbc30dd 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -206,10 +206,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 			      0,  0,  1.f
 			     };		/**< init: identity matrix */
 
-	// print text
-	printf("Extended Kalman Filter Attitude Estimator initialized..\n\n");
-	fflush(stdout);
-
 	int overloadcounter = 19;
 
 	/* Initialize filter */
-- 
cgit v1.2.3


From 2a76b10f7a76273ac3b16d5ac917d40ea28a277b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 22 Nov 2014 15:47:10 +0100
Subject: mc attitude controller: Less verbose

---
 src/modules/mc_att_control/mc_att_control_main.cpp | 2 --
 1 file changed, 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 19c10198c..81a13edb8 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -721,8 +721,6 @@ MulticopterAttitudeControl::task_main_trampoline(int argc, char *argv[])
 void
 MulticopterAttitudeControl::task_main()
 {
-	warnx("started");
-	fflush(stdout);
 
 	/*
 	 * do subscriptions
-- 
cgit v1.2.3


From 4c281030bb782f852600daf217d7a75b23489bbc Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 22 Nov 2014 15:47:54 +0100
Subject: position controller main: Less verbose

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 --
 1 file changed, 2 deletions(-)

(limited to 'src')

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 769f84cb6..5918d6bc5 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -857,10 +857,8 @@ MulticopterPositionControl::control_auto(float dt)
 void
 MulticopterPositionControl::task_main()
 {
-	warnx("started");
 
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(_mavlink_fd, "[mpc] started");
 
 	/*
 	 * do subscriptions
-- 
cgit v1.2.3


From ace6c3fe409f4b70e75bb762e0ba6e0a574a7277 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 22 Nov 2014 15:49:03 +0100
Subject: INAV: Less verbose

---
 .../position_estimator_inav/position_estimator_inav_main.c        | 8 +++-----
 1 file changed, 3 insertions(+), 5 deletions(-)

(limited to 'src')

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index c46d22733..e736a86d7 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -161,7 +161,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 			thread_should_exit = true;
 
 		} else {
-			warnx("app not started");
+			warnx("not started");
 		}
 
 		exit(0);
@@ -169,10 +169,10 @@ int position_estimator_inav_main(int argc, char *argv[])
 
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
-			warnx("app is running");
+			warnx("is running");
 
 		} else {
-			warnx("app not started");
+			warnx("not started");
 		}
 
 		exit(0);
@@ -210,10 +210,8 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
  ****************************************************************************/
 int position_estimator_inav_thread_main(int argc, char *argv[])
 {
-	warnx("started");
 	int mavlink_fd;
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	mavlink_log_info(mavlink_fd, "[inav] started");
 
 	float x_est[2] = { 0.0f, 0.0f };	// pos, vel
 	float y_est[2] = { 0.0f, 0.0f };	// pos, vel
-- 
cgit v1.2.3


From 138c25ec74f08db6f6759053d0200058a794c196 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 22 Nov 2014 15:49:34 +0100
Subject: dataman: less verbose, fix code style

---
 src/modules/dataman/dataman.c | 14 +++++---------
 1 file changed, 5 insertions(+), 9 deletions(-)

(limited to 'src')

diff --git a/src/modules/dataman/dataman.c b/src/modules/dataman/dataman.c
index b2355d4d8..705e54be3 100644
--- a/src/modules/dataman/dataman.c
+++ b/src/modules/dataman/dataman.c
@@ -629,9 +629,6 @@ task_main(int argc, char *argv[])
 {
 	work_q_item_t *work;
 
-	/* inform about start */
-	warnx("Initializing..");
-
 	/* Initialize global variables */
 	g_key_offsets[0] = 0;
 
@@ -694,16 +691,15 @@ task_main(int argc, char *argv[])
 		if (sys_restart_val == DM_INIT_REASON_POWER_ON) {
 			warnx("Power on restart");
 			_restart(DM_INIT_REASON_POWER_ON);
-		}
-		else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
+		} else if (sys_restart_val == DM_INIT_REASON_IN_FLIGHT) {
 			warnx("In flight restart");
 			_restart(DM_INIT_REASON_IN_FLIGHT);
-		}
-		else
+		} else {
 			warnx("Unknown restart");
-	}
-	else
+		}
+	} else {
 		warnx("Unknown restart");
+	}
 
 	/* We use two file descriptors, one for the caller context and one for the worker thread */
 	/* They are actually the same but we need to some way to reject caller request while the */
-- 
cgit v1.2.3