From 29d78367846ebf7834ecd87b2cf528573c3fcdd8 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Wed, 14 Aug 2013 10:53:47 +0200
Subject: [PATCH 01/23] RGBled fixes: options and off after rgb working now

---
 src/drivers/rgbled/rgbled.cpp | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index 44aa922e6e..236f138a74 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -195,8 +195,8 @@ RGBLED::setMode(enum ledModes new_mode)
 		default:
 			if (running) {
 				running = false;
-				set_on(false);
 			}
+			set_on(false);
 			mode = LED_MODE_OFF;
 			break;
 	}
@@ -443,7 +443,8 @@ rgbled_main(int argc, char *argv[])
 	int rgbledadr = ADDR; /* 7bit */
 
 	int ch;
-	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc-1, &argv[1], "a:b:")) != EOF) {
 		switch (ch) {
 		case 'a':
 			rgbledadr = strtol(optarg, NULL, 0);
@@ -455,9 +456,8 @@ rgbled_main(int argc, char *argv[])
 			rgbled_usage();
 		}
 	}
-	argc -= optind;
-	argv += optind;
-	const char *verb = argv[0];
+
+	const char *verb = argv[1];
 
 	if (!strcmp(verb, "start")) {
 		if (g_rgbled != nullptr)
@@ -523,13 +523,13 @@ rgbled_main(int argc, char *argv[])
 		if (fd == -1) {
 			errx(1, "Unable to open " RGBLED_DEVICE_PATH);
 		}
-		if (argc < 4) {
+		if (argc < 5) {
 			errx(1, "Usage: rgbled rgb <red> <green> <blue>");
 		}
 		struct RGBLEDSet v;
-		v.red   = strtol(argv[1], NULL, 0);
-		v.green = strtol(argv[2], NULL, 0);
-		v.blue  = strtol(argv[3], NULL, 0);
+		v.red   = strtol(argv[2], NULL, 0);
+		v.green = strtol(argv[3], NULL, 0);
+		v.blue  = strtol(argv[4], NULL, 0);
 		int ret = ioctl(fd, RGBLED_SET, (unsigned long)&v);
 		close(fd);
 		exit(ret);

From 75e56285901cdb95f0ebbad62d841fbe9d38c1a1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 15 Aug 2013 12:52:07 +0200
Subject: [PATCH 02/23] Fixed Doxygen

---
 Documentation/Doxyfile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile
index 4802ef8189..fd997baaa0 100644
--- a/Documentation/Doxyfile
+++ b/Documentation/Doxyfile
@@ -569,7 +569,7 @@ WARN_LOGFILE           = doxy.log
 # directories like "/usr/src/myproject". Separate the files or directories
 # with spaces.
 
-INPUT                  = ../apps
+INPUT                  = ../src
 
 # This tag can be used to specify the character encoding of the source files
 # that doxygen parses. Internally doxygen uses the UTF-8 encoding, which is

From a1e0581facd28c8c3f2797e72f810a6674216aa5 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 15 Aug 2013 12:55:16 +0200
Subject: [PATCH 03/23] Hotfix: More Doxygen fixes

---
 .gitignore             | 4 +++-
 Documentation/Doxyfile | 4 +---
 2 files changed, 4 insertions(+), 4 deletions(-)

diff --git a/.gitignore b/.gitignore
index 0372b60c80..6ae5baeccb 100644
--- a/.gitignore
+++ b/.gitignore
@@ -24,4 +24,6 @@ Firmware.sublime-workspace
 Images/*.bin
 Images/*.px4
 mavlink/include/mavlink/v0.9/
-NuttX
\ No newline at end of file
+/NuttX
+/Documentation/doxy.log
+/Documentation/html/
\ No newline at end of file
diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile
index fd997baaa0..f45fe8b211 100644
--- a/Documentation/Doxyfile
+++ b/Documentation/Doxyfile
@@ -599,9 +599,7 @@ RECURSIVE              = YES
 # excluded from the INPUT source files. This way you can easily exclude a
 # subdirectory from a directory tree whose root is specified with the INPUT tag.
 
-EXCLUDE                = ../dist/ \
-						 ../docs/html/ \
-						 html
+EXCLUDE                = ../src/mathlib/CMSIS
 
 # The EXCLUDE_SYMLINKS tag can be used select whether or not files or
 # directories that are symbolic links (a Unix filesystem feature) are excluded

From ca877e0bc480e6f28e58a492a64b99a8afe59a21 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 15 Aug 2013 13:00:42 +0200
Subject: [PATCH 04/23] Fixed file exclude

---
 Documentation/Doxyfile | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile
index f45fe8b211..6283cfc9c5 100644
--- a/Documentation/Doxyfile
+++ b/Documentation/Doxyfile
@@ -599,7 +599,7 @@ RECURSIVE              = YES
 # excluded from the INPUT source files. This way you can easily exclude a
 # subdirectory from a directory tree whose root is specified with the INPUT tag.
 
-EXCLUDE                = ../src/mathlib/CMSIS
+EXCLUDE                = ../src/modules/mathlib/CMSIS
 
 # The EXCLUDE_SYMLINKS tag can be used select whether or not files or
 # directories that are symbolic links (a Unix filesystem feature) are excluded

From 74d519d9d0560cd0f54ba2667c8c198d1c933fe3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 15 Aug 2013 13:38:59 +0200
Subject: [PATCH 05/23] Hotfix: Fixed Doxygen tags for uORB topics

---
 .gitignore                                            |  3 ++-
 src/modules/uORB/objects_common.cpp                   |  4 ++++
 src/modules/uORB/topics/actuator_controls.h           |  9 +++++++++
 src/modules/uORB/topics/actuator_controls_effective.h |  9 +++++++++
 src/modules/uORB/topics/actuator_outputs.h            |  9 +++++++++
 src/modules/uORB/topics/debug_key_value.h             |  1 +
 src/modules/uORB/topics/esc_status.h                  | 11 ++++++-----
 src/modules/uORB/topics/mission.h                     | 10 +++++-----
 src/modules/uORB/topics/offboard_control_setpoint.h   | 10 +++++-----
 src/modules/uORB/topics/omnidirectional_flow.h        |  1 +
 src/modules/uORB/topics/parameter_update.h            |  9 +++++++++
 src/modules/uORB/topics/rc_channels.h                 | 10 +++++-----
 src/modules/uORB/topics/sensor_combined.h             | 10 +++++-----
 src/modules/uORB/topics/subsystem_info.h              |  8 ++++----
 src/modules/uORB/topics/telemetry_status.h            |  9 +++++++++
 src/modules/uORB/topics/vehicle_attitude.h            |  1 +
 src/modules/uORB/topics/vehicle_command.h             |  9 ++++-----
 .../uORB/topics/vehicle_global_position_set_triplet.h |  4 ++--
 src/modules/uORB/topics/vehicle_status.h              |  8 ++++----
 19 files changed, 94 insertions(+), 41 deletions(-)

diff --git a/.gitignore b/.gitignore
index 6ae5baeccb..3fdc1bf2a2 100644
--- a/.gitignore
+++ b/.gitignore
@@ -26,4 +26,5 @@ Images/*.px4
 mavlink/include/mavlink/v0.9/
 /NuttX
 /Documentation/doxy.log
-/Documentation/html/
\ No newline at end of file
+/Documentation/html/
+/Documentation/doxygen*objdb*tmp
\ No newline at end of file
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index ae5fc6c61c..301cfa255f 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -37,6 +37,10 @@
  * Common object definitions without a better home.
  */
 
+/**
+ * @defgroup topics List of all uORB topics.
+ */
+
 #include <nuttx/config.h>
 
 #include <drivers/drv_orb_dev.h>
diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h
index b7c4196c0a..a27095be52 100644
--- a/src/modules/uORB/topics/actuator_controls.h
+++ b/src/modules/uORB/topics/actuator_controls.h
@@ -52,11 +52,20 @@
 #define NUM_ACTUATOR_CONTROLS		8
 #define NUM_ACTUATOR_CONTROL_GROUPS	4	/**< for sanity checking */
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct actuator_controls_s {
 	uint64_t timestamp;
 	float	control[NUM_ACTUATOR_CONTROLS];
 };
 
+/**
+ * @}
+ */
+
 /* actuator control sets; this list can be expanded as more controllers emerge */
 ORB_DECLARE(actuator_controls_0);
 ORB_DECLARE(actuator_controls_1);
diff --git a/src/modules/uORB/topics/actuator_controls_effective.h b/src/modules/uORB/topics/actuator_controls_effective.h
index 088c4fc8fe..d7b404ad42 100644
--- a/src/modules/uORB/topics/actuator_controls_effective.h
+++ b/src/modules/uORB/topics/actuator_controls_effective.h
@@ -53,11 +53,20 @@
 #define NUM_ACTUATOR_CONTROLS_EFFECTIVE		NUM_ACTUATOR_CONTROLS
 #define NUM_ACTUATOR_CONTROL_GROUPS_EFFECTIVE	NUM_ACTUATOR_CONTROL_GROUPS	/**< for sanity checking */
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct actuator_controls_effective_s {
 	uint64_t timestamp;
 	float	control_effective[NUM_ACTUATOR_CONTROLS_EFFECTIVE];
 };
 
+/**
+ * @}
+ */
+
 /* actuator control sets; this list can be expanded as more controllers emerge */
 ORB_DECLARE(actuator_controls_effective_0);
 ORB_DECLARE(actuator_controls_effective_1);
diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h
index bbe4290731..30895ca83f 100644
--- a/src/modules/uORB/topics/actuator_outputs.h
+++ b/src/modules/uORB/topics/actuator_outputs.h
@@ -52,12 +52,21 @@
 #define NUM_ACTUATOR_OUTPUTS		16
 #define NUM_ACTUATOR_OUTPUT_GROUPS	4	/**< for sanity checking */
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct actuator_outputs_s {
 	uint64_t timestamp;				/**< output timestamp in us since system boot */
 	float	output[NUM_ACTUATOR_OUTPUTS];		/**< output data, in natural output units */
 	int noutputs;					/**< valid outputs */
 };
 
+/**
+ * @}
+ */
+
 /* actuator output sets; this list can be expanded as more drivers emerge */
 ORB_DECLARE(actuator_outputs_0);
 ORB_DECLARE(actuator_outputs_1);
diff --git a/src/modules/uORB/topics/debug_key_value.h b/src/modules/uORB/topics/debug_key_value.h
index a9d1b83fdc..9253c787db 100644
--- a/src/modules/uORB/topics/debug_key_value.h
+++ b/src/modules/uORB/topics/debug_key_value.h
@@ -47,6 +47,7 @@
 
 /**
  * @addtogroup topics
+ * @{
  */
 
 /**
diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
index 00cf59b28f..11332d7a7a 100644
--- a/src/modules/uORB/topics/esc_status.h
+++ b/src/modules/uORB/topics/esc_status.h
@@ -51,10 +51,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics @{
- */
-
 /**
  * The number of ESCs supported.
  * Current (Q2/2013) we support 8 ESCs,
@@ -76,7 +72,12 @@ enum ESC_CONNECTION_TYPE {
 };
 
 /**
- *
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Electronic speed controller status.
  */
 struct esc_status_s
 {
diff --git a/src/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
index 253f444b3f..978a3383a5 100644
--- a/src/modules/uORB/topics/mission.h
+++ b/src/modules/uORB/topics/mission.h
@@ -46,11 +46,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 enum NAV_CMD {
 	NAV_CMD_WAYPOINT = 0,
 	NAV_CMD_LOITER_TURN_COUNT,
@@ -61,6 +56,11 @@ enum NAV_CMD {
 	NAV_CMD_TAKEOFF
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 /**
  * Global position setpoint in WGS84 coordinates.
  *
diff --git a/src/modules/uORB/topics/offboard_control_setpoint.h b/src/modules/uORB/topics/offboard_control_setpoint.h
index 2e895c59c4..7901b930af 100644
--- a/src/modules/uORB/topics/offboard_control_setpoint.h
+++ b/src/modules/uORB/topics/offboard_control_setpoint.h
@@ -43,11 +43,6 @@
 #include <stdint.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 /**
  * Off-board control inputs.
  * 
@@ -66,6 +61,11 @@ enum OFFBOARD_CONTROL_MODE
 	OFFBOARD_CONTROL_MODE_MULTIROTOR_SIMPLE = 7, /**< roll / pitch rotated aligned to the takeoff orientation, throttle stabilized, yaw pos */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct offboard_control_setpoint_s {
 	uint64_t timestamp;
 
diff --git a/src/modules/uORB/topics/omnidirectional_flow.h b/src/modules/uORB/topics/omnidirectional_flow.h
index 8f4be3b3fe..a6ad8a1315 100644
--- a/src/modules/uORB/topics/omnidirectional_flow.h
+++ b/src/modules/uORB/topics/omnidirectional_flow.h
@@ -46,6 +46,7 @@
 
 /**
  * @addtogroup topics
+ * @{
  */
 
 /**
diff --git a/src/modules/uORB/topics/parameter_update.h b/src/modules/uORB/topics/parameter_update.h
index 300e895c7d..68964deb0a 100644
--- a/src/modules/uORB/topics/parameter_update.h
+++ b/src/modules/uORB/topics/parameter_update.h
@@ -42,11 +42,20 @@
 #include <stdint.h>
 #include "../uORB.h"
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct parameter_update_s {
 	/** time at which the latest parameter was updated */
 	uint64_t	timestamp;
 };
 
+/**
+ * @}
+ */
+
 ORB_DECLARE(parameter_update);
 
 #endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 9dd54df915..e69335b3d8 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -45,11 +45,6 @@
 #include <stdint.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 /**
  * The number of RC channel inputs supported.
  * Current (Q1/2013) radios support up to 18 channels,
@@ -83,6 +78,11 @@ enum RC_CHANNELS_FUNCTION
   RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct rc_channels_s {
 
 	uint64_t timestamp;                 /**< In microseconds since boot time. */
diff --git a/src/modules/uORB/topics/sensor_combined.h b/src/modules/uORB/topics/sensor_combined.h
index 9a76b51829..ad164555e2 100644
--- a/src/modules/uORB/topics/sensor_combined.h
+++ b/src/modules/uORB/topics/sensor_combined.h
@@ -46,17 +46,17 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 enum MAGNETOMETER_MODE {
 	MAGNETOMETER_MODE_NORMAL = 0,
 	MAGNETOMETER_MODE_POSITIVE_BIAS,
 	MAGNETOMETER_MODE_NEGATIVE_BIAS
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 /**
  * Sensor readings in raw and SI-unit form.
  *
diff --git a/src/modules/uORB/topics/subsystem_info.h b/src/modules/uORB/topics/subsystem_info.h
index c415e832e0..cfe0bf69e9 100644
--- a/src/modules/uORB/topics/subsystem_info.h
+++ b/src/modules/uORB/topics/subsystem_info.h
@@ -50,10 +50,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- */
-
 enum SUBSYSTEM_TYPE
 {
 	SUBSYSTEM_TYPE_GYRO = 1,
@@ -75,6 +71,10 @@ enum SUBSYSTEM_TYPE
 	SUBSYSTEM_TYPE_RANGEFINDER = 131072
 };
 
+/**
+ * @addtogroup topics
+ */
+
 /**
  * State of individual sub systems
  */
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index f30852de5b..828fb31cc7 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -50,6 +50,11 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
     TELEMETRY_STATUS_RADIO_TYPE_WIRE
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 struct telemetry_status_s {
 	uint64_t timestamp;
     enum TELEMETRY_STATUS_RADIO_TYPE type;  /**< type of the radio hardware     */
@@ -62,6 +67,10 @@ struct telemetry_status_s {
     uint8_t txbuf;              /**< how full the tx buffer is as a percentage  */
 };
 
+/**
+ * @}
+ */
+
 ORB_DECLARE(telemetry_status);
 
 #endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file
diff --git a/src/modules/uORB/topics/vehicle_attitude.h b/src/modules/uORB/topics/vehicle_attitude.h
index c31c81d0ce..4380a5ee77 100755
--- a/src/modules/uORB/topics/vehicle_attitude.h
+++ b/src/modules/uORB/topics/vehicle_attitude.h
@@ -48,6 +48,7 @@
 
 /**
  * @addtogroup topics
+ * @{
  */
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index fac571659d..31ff014de9 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -45,11 +45,6 @@
 #include <stdint.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics
- * @{
- */
-
 /**
  * Commands for commander app.
  *
@@ -110,6 +105,10 @@ enum VEHICLE_CMD_RESULT
 	VEHICLE_CMD_RESULT_ENUM_END=5, /*  | */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
 
 struct vehicle_command_s
 {
diff --git a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
index 318abba89f..8516b263fc 100644
--- a/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
+++ b/src/modules/uORB/topics/vehicle_global_position_set_triplet.h
@@ -60,8 +60,8 @@
  */
 struct vehicle_global_position_set_triplet_s
 {
-	bool previous_valid;
-	bool next_valid;
+	bool previous_valid;					/**< flag indicating previous position is valid */
+	bool next_valid;					/**< flag indicating next position is valid */
 
 	struct vehicle_global_position_setpoint_s previous;
 	struct vehicle_global_position_setpoint_s current;
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index c7c1048f60..94068a9acc 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -54,10 +54,6 @@
 #include <stdbool.h>
 #include "../uORB.h"
 
-/**
- * @addtogroup topics @{
- */
-
 /* State Machine */
 typedef enum
 {
@@ -137,6 +133,10 @@ enum VEHICLE_BATTERY_WARNING {
     VEHICLE_BATTERY_WARNING_ALERT            /**< aleting of low voltage 2. stage */
 };
 
+/**
+ * @addtogroup topics
+ * @{
+ */
 
 /**
  * state machine / state of vehicle.

From b83d07e94a0db879ff59a45f69450152ae330150 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 15 Aug 2013 13:47:36 +0200
Subject: [PATCH 06/23] Hotfix: Excluding codegen code

---
 Documentation/Doxyfile | 3 ++-
 1 file changed, 2 insertions(+), 1 deletion(-)

diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile
index 6283cfc9c5..65274d4a4f 100644
--- a/Documentation/Doxyfile
+++ b/Documentation/Doxyfile
@@ -599,7 +599,8 @@ RECURSIVE              = YES
 # excluded from the INPUT source files. This way you can easily exclude a
 # subdirectory from a directory tree whose root is specified with the INPUT tag.
 
-EXCLUDE                = ../src/modules/mathlib/CMSIS
+EXCLUDE                = ../src/modules/mathlib/CMSIS \
+			 ../src/modules/attitude_estimator_ekf/codegen
 
 # The EXCLUDE_SYMLINKS tag can be used select whether or not files or
 # directories that are symbolic links (a Unix filesystem feature) are excluded

From ab80b0e273d5b2d795c8b5f470f773052cbaeedb Mon Sep 17 00:00:00 2001
From: Jean Cyr <jcyr@dillobits.com>
Date: Thu, 15 Aug 2013 19:51:36 -0400
Subject: [PATCH 07/23] Doxygenate and style dsm.c

One file a day... this'll take a while!
---
 src/modules/px4iofirmware/dsm.c | 412 +++++++++++++++++++-------------
 1 file changed, 243 insertions(+), 169 deletions(-)

diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index b2c0db4254..e014b0a519 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -48,158 +48,55 @@
 
 #include <drivers/drv_hrt.h>
 
-#define DEBUG
-
 #include "px4io.h"
 
+//! The DSM dsm frame size in bytes
 #define DSM_FRAME_SIZE		16
+//! Maximum supported DSM channels
 #define DSM_FRAME_CHANNELS	7
 
-static int dsm_fd = -1;
-
-static hrt_abstime last_rx_time;
-static hrt_abstime last_frame_time;
-
-static uint8_t	frame[DSM_FRAME_SIZE];
-
-static unsigned partial_frame_count;
-static unsigned	channel_shift;
-
+//! File handle to the DSM UART
+int dsm_fd = -1;
+//! Timestamp when we last received
+hrt_abstime dsm_last_rx_time;
+//! Timestamp for start of last dsm_frame
+hrt_abstime dsm_last_frame_time;
+//! DSM dsm_frame receive buffer
+uint8_t	dsm_frame[DSM_FRAME_SIZE];
+//! Count of bytes received for current dsm_frame
+unsigned dsm_partial_frame_count;
+//! Channel resolution, 0=unknown, 1=10 bit, 2=11 bit
+unsigned dsm_channel_shift;
+//! Count of incomplete DSM frames
 unsigned dsm_frame_drops;
 
-static bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
-static void dsm_guess_format(bool reset);
-static bool dsm_decode(hrt_abstime now, uint16_t *values, uint16_t *num_values);
-
-int
-dsm_init(const char *device)
-{
-	if (dsm_fd < 0)
-		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
-
-	if (dsm_fd >= 0) {
-		struct termios t;
-
-		/* 115200bps, no parity, one stop bit */
-		tcgetattr(dsm_fd, &t);
-		cfsetspeed(&t, 115200);
-		t.c_cflag &= ~(CSTOPB | PARENB);
-		tcsetattr(dsm_fd, TCSANOW, &t);
-
-		/* initialise the decoder */
-		partial_frame_count = 0;
-		last_rx_time = hrt_absolute_time();
-
-		/* reset the format detector */
-		dsm_guess_format(true);
-
-		debug("DSM: ready");
-
-	} else {
-		debug("DSM: open failed");
-	}
-
-	return dsm_fd;
-}
-
-void
-dsm_bind(uint16_t cmd, int pulses)
-{
-	const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
-
-	if (dsm_fd < 0)
-		return;
-
-	switch (cmd) {
-	case dsm_bind_power_down:
-		// power down DSM satellite
-		POWER_RELAY1(0);
-		break;
-	case dsm_bind_power_up:
-		POWER_RELAY1(1);
-		dsm_guess_format(true);
-		break;
-	case dsm_bind_set_rx_out:
-		stm32_configgpio(usart1RxAsOutp); 
-		break;
-	case dsm_bind_send_pulses:
-		for (int i = 0; i < pulses; i++) {
-			stm32_gpiowrite(usart1RxAsOutp, false);
-			up_udelay(25);
-			stm32_gpiowrite(usart1RxAsOutp, true);
-			up_udelay(25);
-		}
-		break;
-	case dsm_bind_reinit_uart:
-		// Restore USART rx pin
-		stm32_configgpio(GPIO_USART1_RX);
-		break;
-	}
-}
-
-bool
-dsm_input(uint16_t *values, uint16_t *num_values)
-{
-	ssize_t		ret;
-	hrt_abstime	now;
-
-	/*
-	 * The DSM* protocol doesn't provide any explicit framing,
-	 * so we detect frame boundaries by the inter-frame delay.
-	 *
-	 * The minimum frame spacing is 11ms; with 16 bytes at 115200bps
-	 * frame transmission time is ~1.4ms.
-	 *
-	 * We expect to only be called when bytes arrive for processing,
-	 * and if an interval of more than 5ms passes between calls,
-	 * the first byte we read will be the first byte of a frame.
-	 *
-	 * In the case where byte(s) are dropped from a frame, this also
-	 * provides a degree of protection. Of course, it would be better
-	 * if we didn't drop bytes...
-	 */
-	now = hrt_absolute_time();
-
-	if ((now - last_rx_time) > 5000) {
-		if (partial_frame_count > 0) {
-			dsm_frame_drops++;
-			partial_frame_count = 0;
-		}
-	}
-
-	/*
-	 * Fetch bytes, but no more than we would need to complete
-	 * the current frame.
-	 */
-	ret = read(dsm_fd, &frame[partial_frame_count], DSM_FRAME_SIZE - partial_frame_count);
-
-	/* if the read failed for any reason, just give up here */
-	if (ret < 1)
-		return false;
-
-	last_rx_time = now;
-
-	/*
-	 * Add bytes to the current frame
-	 */
-	partial_frame_count += ret;
-
-	/*
-	 * If we don't have a full frame, return
-	 */
-	if (partial_frame_count < DSM_FRAME_SIZE)
-		return false;
-
-	/*
-	 * Great, it looks like we might have a frame.  Go ahead and
-	 * decode it.
-	 */
-	partial_frame_count = 0;
-	return dsm_decode(now, values, num_values);
-}
-
-static bool
-dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
+/**
+ * Attempt to decode a single channel raw channel datum
+ *
+ * The DSM* protocol doesn't provide any explicit framing,
+ * so we detect dsm_frame boundaries by the inter-dsm_frame delay.
+ *
+ * The minimum dsm_frame spacing is 11ms; with 16 bytes at 115200bps
+ * dsm_frame transmission time is ~1.4ms.
+ *
+ * We expect to only be called when bytes arrive for processing,
+ * and if an interval of more than 5ms passes between calls,
+ * the first byte we read will be the first byte of a dsm_frame.
+ *
+ * In the case where byte(s) are dropped from a dsm_frame, this also
+ * provides a degree of protection. Of course, it would be better
+ * if we didn't drop bytes...
+ *
+ * Upon receiving a full dsm_frame we attempt to decode it
+ *
+ * @param[in] raw 16 bit raw channel value from dsm_frame
+ * @param[in] shift position of channel number in raw data
+ * @param[out] channel pointer to returned channel number
+ * @param[out] value pointer to returned channel value
+ * @return true=raw value successfully decoded
+ */
+bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
+bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
 {
 
 	if (raw == 0xffff)
@@ -215,8 +112,13 @@ dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *va
 	return true;
 }
 
-static void
-dsm_guess_format(bool reset)
+/**
+ * Attempt to guess if receiving 10 or 11 bit channel values
+ *
+ * @param[in] reset true=reset the 10/11 bit state to unknow
+ */
+void dsm_guess_format(bool reset);
+void dsm_guess_format(bool reset)
 {
 	static uint32_t	cs10;
 	static uint32_t	cs11;
@@ -227,14 +129,14 @@ dsm_guess_format(bool reset)
 		cs10 = 0;
 		cs11 = 0;
 		samples = 0;
-		channel_shift = 0;
+		dsm_channel_shift = 0;
 		return;
 	}
 
-	/* scan the channels in the current frame in both 10- and 11-bit mode */
+	/* scan the channels in the current dsm_frame in both 10- and 11-bit mode */
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
-		uint8_t *dp = &frame[2 + (2 * i)];
+		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
 		unsigned channel, value;
 
@@ -245,10 +147,10 @@ dsm_guess_format(bool reset)
 		if (dsm_decode_channel(raw, 11, &channel, &value) && (channel < 31))
 			cs11 |= (1 << channel);
 
-		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-frame format */
+		/* XXX if we cared, we could look for the phase bit here to decide 1 vs. 2-dsm_frame format */
 	}
 
-	/* wait until we have seen plenty of frames - 2 should normally be enough */
+	/* wait until we have seen plenty of frames - 5 should normally be enough */
 	if (samples++ < 5)
 		return;
 
@@ -284,13 +186,13 @@ dsm_guess_format(bool reset)
 	}
 
 	if ((votes11 == 1) && (votes10 == 0)) {
-		channel_shift = 11;
+		dsm_channel_shift = 11;
 		debug("DSM: 11-bit format");
 		return;
 	}
 
 	if ((votes10 == 1) && (votes11 == 0)) {
-		channel_shift = 10;
+		dsm_channel_shift = 10;
 		debug("DSM: 10-bit format");
 		return;
 	}
@@ -300,27 +202,129 @@ dsm_guess_format(bool reset)
 	dsm_guess_format(true);
 }
 
-static bool
-dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
+/**
+ * Initialize the DSM receive functionality
+ *
+ * Open the UART for receiving DSM frames and configure it appropriately
+ *
+ * @param[in] device Device name of DSM UART
+ */
+int dsm_init(const char *device)
 {
+	if (dsm_fd < 0)
+		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
 
+	if (dsm_fd >= 0) {
+
+		struct termios t;
+
+		/* 115200bps, no parity, one stop bit */
+		tcgetattr(dsm_fd, &t);
+		cfsetspeed(&t, 115200);
+		t.c_cflag &= ~(CSTOPB | PARENB);
+		tcsetattr(dsm_fd, TCSANOW, &t);
+
+		/* initialise the decoder */
+		dsm_partial_frame_count = 0;
+		dsm_last_rx_time = hrt_absolute_time();
+
+		/* reset the format detector */
+		dsm_guess_format(true);
+
+		debug("DSM: ready");
+
+	} else {
+
+		debug("DSM: open failed");
+
+	}
+
+	return dsm_fd;
+}
+
+/**
+ * Handle DSM satellite receiver bind mode handler
+ *
+ * @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart
+ * @param[in] pulses Number of pulses for dsm_bind_send_pulses command
+ */
+void dsm_bind(uint16_t cmd, int pulses)
+{
+	const uint32_t usart1RxAsOutp =
+		GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
+
+	if (dsm_fd < 0)
+		return;
+
+	switch (cmd) {
+
+	case dsm_bind_power_down:
+
+		//! power down DSM satellite
+		POWER_RELAY1(0);
+		break;
+
+	case dsm_bind_power_up:
+
+		//! power up DSM satellite
+		POWER_RELAY1(1);
+		dsm_guess_format(true);
+		break;
+
+	case dsm_bind_set_rx_out:
+
+		//! Set UART RX pin to active output mode
+		stm32_configgpio(usart1RxAsOutp);
+		break;
+
+	case dsm_bind_send_pulses:
+
+		//! Pulse RX pin a number of times
+		for (int i = 0; i < pulses; i++) {
+			stm32_gpiowrite(usart1RxAsOutp, false);
+			up_udelay(25);
+			stm32_gpiowrite(usart1RxAsOutp, true);
+			up_udelay(25);
+		}
+		break;
+
+	case dsm_bind_reinit_uart:
+
+		//! Restore USART RX pin to RS232 receive mode
+		stm32_configgpio(GPIO_USART1_RX);
+		break;
+
+	}
+}
+
+/**
+ * Decode the entire dsm_frame (all contained channels)
+ *
+ * @param[in] frame_time timestamp when this dsm_frame was received. Used to detect RX loss in order to reset 10/11 bit guess.
+ * @param[out] values pointer to per channel array of decoded values
+ * @param[out] num_values pointer to number of raw channel values returned
+ * @return true=dsm_frame successfully decoded
+ */
+bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values);
+bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
+{
 	/*
-		debug("DSM frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
-		      frame[0], frame[1], frame[2], frame[3], frame[4], frame[5], frame[6], frame[7],
-		      frame[8], frame[9], frame[10], frame[11], frame[12], frame[13], frame[14], frame[15]);
+	debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
+		dsm_frame[0], dsm_frame[1], dsm_frame[2], dsm_frame[3], dsm_frame[4], dsm_frame[5], dsm_frame[6], dsm_frame[7],
+		dsm_frame[8], dsm_frame[9], dsm_frame[10], dsm_frame[11], dsm_frame[12], dsm_frame[13], dsm_frame[14], dsm_frame[15]);
 	*/
 	/*
 	 * If we have lost signal for at least a second, reset the
 	 * format guessing heuristic.
 	 */
-	if (((frame_time - last_frame_time) > 1000000) && (channel_shift != 0))
+	if (((frame_time - dsm_last_frame_time) > 1000000) && (dsm_channel_shift != 0))
 		dsm_guess_format(true);
 
-	/* we have received something we think is a frame */
-	last_frame_time = frame_time;
+	/* we have received something we think is a dsm_frame */
+	dsm_last_frame_time = frame_time;
 
-	/* if we don't know the frame format, update the guessing state machine */
-	if (channel_shift == 0) {
+	/* if we don't know the dsm_frame format, update the guessing state machine */
+	if (dsm_channel_shift == 0) {
 		dsm_guess_format(false);
 		return false;
 	}
@@ -332,17 +336,17 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	 * Each channel is a 16-bit unsigned value containing either a 10-
 	 * or 11-bit channel value and a 4-bit channel number, shifted
 	 * either 10 or 11 bits. The MSB may also be set to indicate the
-	 * second frame in variants of the protocol where more than
+	 * second dsm_frame in variants of the protocol where more than
 	 * seven channels are being transmitted.
 	 */
 
 	for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) {
 
-		uint8_t *dp = &frame[2 + (2 * i)];
+		uint8_t *dp = &dsm_frame[2 + (2 * i)];
 		uint16_t raw = (dp[0] << 8) | dp[1];
 		unsigned channel, value;
 
-		if (!dsm_decode_channel(raw, channel_shift, &channel, &value))
+		if (!dsm_decode_channel(raw, dsm_channel_shift, &channel, &value))
 			continue;
 
 		/* ignore channels out of range */
@@ -354,7 +358,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 			*num_values = channel + 1;
 
 		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
-		if (channel_shift == 11)
+		if (dsm_channel_shift == 11)
 			value /= 2;
 
 		value += 998;
@@ -385,7 +389,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		values[channel] = value;
 	}
 
-	if (channel_shift == 11)
+	if (dsm_channel_shift == 11)
 		*num_values |= 0x8000;
 
 	/*
@@ -393,3 +397,73 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 	 */
 	return true;
 }
+
+/**
+ * Called periodically to check for input data from the DSM UART
+ *
+ * The DSM* protocol doesn't provide any explicit framing,
+ * so we detect dsm_frame boundaries by the inter-dsm_frame delay.
+ *
+ * The minimum dsm_frame spacing is 11ms; with 16 bytes at 115200bps
+ * dsm_frame transmission time is ~1.4ms.
+ *
+ * We expect to only be called when bytes arrive for processing,
+ * and if an interval of more than 5ms passes between calls,
+ * the first byte we read will be the first byte of a dsm_frame.
+ *
+ * In the case where byte(s) are dropped from a dsm_frame, this also
+ * provides a degree of protection. Of course, it would be better
+ * if we didn't drop bytes...
+ *
+ * Upon receiving a full dsm_frame we attempt to decode it
+ *
+ * @param[out] values pointer to per channel array of decoded values
+ * @param[out] num_values pointer to number of raw channel values returned
+ * @return true=decode raw channel values updated
+ */
+bool dsm_input(uint16_t *values, uint16_t *num_values)
+{
+	ssize_t		ret;
+	hrt_abstime	now;
+
+	/*
+	 */
+	now = hrt_absolute_time();
+
+	if ((now - dsm_last_rx_time) > 5000) {
+		if (dsm_partial_frame_count > 0) {
+			dsm_frame_drops++;
+			dsm_partial_frame_count = 0;
+		}
+	}
+
+	/*
+	 * Fetch bytes, but no more than we would need to complete
+	 * the current dsm_frame.
+	 */
+	ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
+
+	/* if the read failed for any reason, just give up here */
+	if (ret < 1)
+		return false;
+
+	dsm_last_rx_time = now;
+
+	/*
+	 * Add bytes to the current dsm_frame
+	 */
+	dsm_partial_frame_count += ret;
+
+	/*
+	 * If we don't have a full dsm_frame, return
+	 */
+	if (dsm_partial_frame_count < DSM_FRAME_SIZE)
+		return false;
+
+	/*
+	 * Great, it looks like we might have a dsm_frame.  Go ahead and
+	 * decode it.
+	 */
+	dsm_partial_frame_count = 0;
+	return dsm_decode(now, values, num_values);
+}

From 6113be111e84a57715f3f3bfe81077bf1b267e52 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 Aug 2013 14:25:21 +0200
Subject: [PATCH 08/23] Hotfix: Do not create bug/test lists, adding noise

---
 Documentation/Doxyfile | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/Documentation/Doxyfile b/Documentation/Doxyfile
index 65274d4a4f..2417028114 100644
--- a/Documentation/Doxyfile
+++ b/Documentation/Doxyfile
@@ -429,19 +429,19 @@ SORT_BY_SCOPE_NAME     = NO
 # disable (NO) the todo list. This list is created by putting \todo
 # commands in the documentation.
 
-GENERATE_TODOLIST      = YES
+GENERATE_TODOLIST      = NO
 
 # The GENERATE_TESTLIST tag can be used to enable (YES) or
 # disable (NO) the test list. This list is created by putting \test
 # commands in the documentation.
 
-GENERATE_TESTLIST      = YES
+GENERATE_TESTLIST      = NO
 
 # The GENERATE_BUGLIST tag can be used to enable (YES) or
 # disable (NO) the bug list. This list is created by putting \bug
 # commands in the documentation.
 
-GENERATE_BUGLIST       = YES
+GENERATE_BUGLIST       = NO
 
 # The GENERATE_DEPRECATEDLIST tag can be used to enable (YES) or
 # disable (NO) the deprecated list. This list is created by putting

From 05e4c086cecf5fa13cac80d0d9724f1b6bac431c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 Aug 2013 16:24:44 +0200
Subject: [PATCH 09/23] Added orientation support and detection to the L3GD20/H
 driver to support the different variants in use

---
 src/drivers/l3gd20/l3gd20.cpp | 55 ++++++++++++++++++++++++++++++++---
 1 file changed, 51 insertions(+), 4 deletions(-)

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 42a0c264c4..05739f04ff 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -71,6 +71,12 @@
 #endif
 static const int ERROR = -1;
 
+/* Orientation on board */
+#define SENSOR_BOARD_ROTATION_000_DEG	0
+#define SENSOR_BOARD_ROTATION_090_DEG	1
+#define SENSOR_BOARD_ROTATION_180_DEG	2
+#define SENSOR_BOARD_ROTATION_270_DEG	3
+
 /* SPI protocol address bits */
 #define DIR_READ				(1<<7)
 #define DIR_WRITE				(0<<7)
@@ -186,6 +192,7 @@ private:
 
 	unsigned		_current_rate;
 	unsigned		_current_range;
+	unsigned		_orientation;
 
 	perf_counter_t		_sample_perf;
 
@@ -283,6 +290,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	_gyro_topic(-1),
 	_current_rate(0),
 	_current_range(0),
+	_orientation(SENSOR_BOARD_ROTATION_270_DEG),
         _sample_perf(perf_alloc(PC_ELAPSED, "l3gd20_read")),
         _gyro_filter_x(250, 30),
         _gyro_filter_y(250, 30),
@@ -363,8 +371,23 @@ L3GD20::probe()
 	(void)read_reg(ADDR_WHO_AM_I);
 
 	/* verify that the device is attached and functioning, accept L3GD20 and L3GD20H */
-	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM || read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H)
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM) {
+
+		#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
+		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+			_orientation = SENSOR_BOARD_ROTATION_270_DEG;
+		#else
+			#error This driver needs a board selection, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+		#endif
 		return OK;
+	}
+
+
+	if (read_reg(ADDR_WHO_AM_I) == WHO_I_AM_H) {
+		_orientation = SENSOR_BOARD_ROTATION_180_DEG;
+		return OK;
+	}
 
 	return -EIO;
 }
@@ -717,9 +740,33 @@ L3GD20::measure()
 	 */
 	report->timestamp = hrt_absolute_time();
 	
-	/* swap x and y and negate y */
-	report->x_raw = raw_report.y;
-	report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+	switch (_orientation) {
+
+		case SENSOR_BOARD_ROTATION_000_DEG:
+			/* keep axes in place */
+			report->x_raw = raw_report.x;
+			report->y_raw = raw_report.y;
+			break;
+
+		case SENSOR_BOARD_ROTATION_090_DEG:
+			/* swap x and y */
+			report->x_raw = raw_report.y;
+			report->y_raw = raw_report.x;
+			break;
+
+		case SENSOR_BOARD_ROTATION_180_DEG:
+			/* swap x and y and negate both */
+			report->x_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+			report->y_raw = ((raw_report.y == -32768) ? 32767 : -raw_report.y);
+			break;
+
+		case SENSOR_BOARD_ROTATION_270_DEG:
+			/* swap x and y and negate y */
+			report->x_raw = raw_report.y;
+			report->y_raw = ((raw_report.x == -32768) ? 32767 : -raw_report.x);
+			break;
+	}
+
 	report->z_raw = raw_report.z;
 
 	report->x = ((report->x_raw * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale;

From bafc5ea8a1f6f55ebd8c54d673566919162f3f30 Mon Sep 17 00:00:00 2001
From: Jean Cyr <jcyr@dillobits.com>
Date: Fri, 16 Aug 2013 18:35:52 -0400
Subject: [PATCH 10/23] Remoce C++ style Doxygen comments

Replace C++ style comments with C comments
---
 src/modules/px4iofirmware/dsm.c   | 106 ++++++++++++++----------------
 src/modules/px4iofirmware/px4io.h |   8 +--
 2 files changed, 52 insertions(+), 62 deletions(-)

diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index e014b0a519..745cdfa403 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -50,53 +50,44 @@
 
 #include "px4io.h"
 
-//! The DSM dsm frame size in bytes
-#define DSM_FRAME_SIZE		16
-//! Maximum supported DSM channels
-#define DSM_FRAME_CHANNELS	7
+#define DSM_FRAME_SIZE		16		/**<DSM frame size in bytes*/
+#define DSM_FRAME_CHANNELS	7		/**<Max supported DSM channels*/
 
-//! File handle to the DSM UART
-int dsm_fd = -1;
-//! Timestamp when we last received
-hrt_abstime dsm_last_rx_time;
-//! Timestamp for start of last dsm_frame
-hrt_abstime dsm_last_frame_time;
-//! DSM dsm_frame receive buffer
-uint8_t	dsm_frame[DSM_FRAME_SIZE];
-//! Count of bytes received for current dsm_frame
-unsigned dsm_partial_frame_count;
-//! Channel resolution, 0=unknown, 1=10 bit, 2=11 bit
-unsigned dsm_channel_shift;
-//! Count of incomplete DSM frames
-unsigned dsm_frame_drops;
+static int dsm_fd = -1;				/**<File handle to the DSM UART*/
+static hrt_abstime dsm_last_rx_time;		/**<Timestamp when we last received*/
+static hrt_abstime dsm_last_frame_time;		/**<Timestamp for start of last dsm frame*/
+static uint8_t dsm_frame[DSM_FRAME_SIZE];	/**<DSM dsm frame receive buffer*/
+static unsigned dsm_partial_frame_count;	/**<Count of bytes received for current dsm frame*/
+static unsigned dsm_channel_shift;		/**<Channel resolution, 0=unknown, 1=10 bit, 2=11 bit*/
+static unsigned dsm_frame_drops;		/**<Count of incomplete DSM frames*/
 
 /**
  * Attempt to decode a single channel raw channel datum
  *
  * The DSM* protocol doesn't provide any explicit framing,
- * so we detect dsm_frame boundaries by the inter-dsm_frame delay.
+ * so we detect dsm frame boundaries by the inter-dsm frame delay.
  *
- * The minimum dsm_frame spacing is 11ms; with 16 bytes at 115200bps
- * dsm_frame transmission time is ~1.4ms.
+ * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
+ * dsm frame transmission time is ~1.4ms.
  *
  * We expect to only be called when bytes arrive for processing,
  * and if an interval of more than 5ms passes between calls,
- * the first byte we read will be the first byte of a dsm_frame.
+ * the first byte we read will be the first byte of a dsm frame.
  *
- * In the case where byte(s) are dropped from a dsm_frame, this also
+ * In the case where byte(s) are dropped from a dsm frame, this also
  * provides a degree of protection. Of course, it would be better
  * if we didn't drop bytes...
  *
- * Upon receiving a full dsm_frame we attempt to decode it
+ * Upon receiving a full dsm frame we attempt to decode it
  *
- * @param[in] raw 16 bit raw channel value from dsm_frame
+ * @param[in] raw 16 bit raw channel value from dsm frame
  * @param[in] shift position of channel number in raw data
  * @param[out] channel pointer to returned channel number
  * @param[out] value pointer to returned channel value
  * @return true=raw value successfully decoded
  */
-bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value);
-bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
+static bool
+dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigned *value)
 {
 
 	if (raw == 0xffff)
@@ -115,10 +106,10 @@ bool dsm_decode_channel(uint16_t raw, unsigned shift, unsigned *channel, unsigne
 /**
  * Attempt to guess if receiving 10 or 11 bit channel values
  *
- * @param[in] reset true=reset the 10/11 bit state to unknow
+ * @param[in] reset true=reset the 10/11 bit state to unknown
  */
-void dsm_guess_format(bool reset);
-void dsm_guess_format(bool reset)
+static void
+dsm_guess_format(bool reset)
 {
 	static uint32_t	cs10;
 	static uint32_t	cs11;
@@ -209,7 +200,8 @@ void dsm_guess_format(bool reset)
  *
  * @param[in] device Device name of DSM UART
  */
-int dsm_init(const char *device)
+int
+dsm_init(const char *device)
 {
 	if (dsm_fd < 0)
 		dsm_fd = open(device, O_RDONLY | O_NONBLOCK);
@@ -248,7 +240,8 @@ int dsm_init(const char *device)
  * @param[in] cmd commands - dsm_bind_power_down, dsm_bind_power_up, dsm_bind_set_rx_out, dsm_bind_send_pulses, dsm_bind_reinit_uart
  * @param[in] pulses Number of pulses for dsm_bind_send_pulses command
  */
-void dsm_bind(uint16_t cmd, int pulses)
+void
+dsm_bind(uint16_t cmd, int pulses)
 {
 	const uint32_t usart1RxAsOutp =
 		GPIO_OUTPUT | GPIO_CNF_OUTPP | GPIO_MODE_50MHz | GPIO_OUTPUT_SET | GPIO_PORTA | GPIO_PIN10;
@@ -260,26 +253,26 @@ void dsm_bind(uint16_t cmd, int pulses)
 
 	case dsm_bind_power_down:
 
-		//! power down DSM satellite
+		/*power down DSM satellite*/
 		POWER_RELAY1(0);
 		break;
 
 	case dsm_bind_power_up:
 
-		//! power up DSM satellite
+		/*power up DSM satellite*/
 		POWER_RELAY1(1);
 		dsm_guess_format(true);
 		break;
 
 	case dsm_bind_set_rx_out:
 
-		//! Set UART RX pin to active output mode
+		/*Set UART RX pin to active output mode*/
 		stm32_configgpio(usart1RxAsOutp);
 		break;
 
 	case dsm_bind_send_pulses:
 
-		//! Pulse RX pin a number of times
+		/*Pulse RX pin a number of times*/
 		for (int i = 0; i < pulses; i++) {
 			stm32_gpiowrite(usart1RxAsOutp, false);
 			up_udelay(25);
@@ -290,7 +283,7 @@ void dsm_bind(uint16_t cmd, int pulses)
 
 	case dsm_bind_reinit_uart:
 
-		//! Restore USART RX pin to RS232 receive mode
+		/*Restore USART RX pin to RS232 receive mode*/
 		stm32_configgpio(GPIO_USART1_RX);
 		break;
 
@@ -298,15 +291,15 @@ void dsm_bind(uint16_t cmd, int pulses)
 }
 
 /**
- * Decode the entire dsm_frame (all contained channels)
+ * Decode the entire dsm frame (all contained channels)
  *
- * @param[in] frame_time timestamp when this dsm_frame was received. Used to detect RX loss in order to reset 10/11 bit guess.
+ * @param[in] frame_time timestamp when this dsm frame was received. Used to detect RX loss in order to reset 10/11 bit guess.
  * @param[out] values pointer to per channel array of decoded values
  * @param[out] num_values pointer to number of raw channel values returned
- * @return true=dsm_frame successfully decoded
+ * @return true=DSM frame successfully decoded, false=no update
  */
-bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values);
-bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
+static bool
+dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 {
 	/*
 	debug("DSM dsm_frame %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x %02x%02x",
@@ -402,26 +395,23 @@ bool dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
  * Called periodically to check for input data from the DSM UART
  *
  * The DSM* protocol doesn't provide any explicit framing,
- * so we detect dsm_frame boundaries by the inter-dsm_frame delay.
- *
- * The minimum dsm_frame spacing is 11ms; with 16 bytes at 115200bps
- * dsm_frame transmission time is ~1.4ms.
- *
+ * so we detect dsm frame boundaries by the inter-dsm frame delay.
+ * The minimum dsm frame spacing is 11ms; with 16 bytes at 115200bps
+ * dsm frame transmission time is ~1.4ms.
  * We expect to only be called when bytes arrive for processing,
  * and if an interval of more than 5ms passes between calls,
- * the first byte we read will be the first byte of a dsm_frame.
- *
- * In the case where byte(s) are dropped from a dsm_frame, this also
+ * the first byte we read will be the first byte of a dsm frame.
+ * In the case where byte(s) are dropped from a dsm frame, this also
  * provides a degree of protection. Of course, it would be better
  * if we didn't drop bytes...
- *
- * Upon receiving a full dsm_frame we attempt to decode it
+ * Upon receiving a full dsm frame we attempt to decode it.
  *
  * @param[out] values pointer to per channel array of decoded values
  * @param[out] num_values pointer to number of raw channel values returned
- * @return true=decode raw channel values updated
+ * @return true=decoded raw channel values updated, false=no update
  */
-bool dsm_input(uint16_t *values, uint16_t *num_values)
+bool
+dsm_input(uint16_t *values, uint16_t *num_values)
 {
 	ssize_t		ret;
 	hrt_abstime	now;
@@ -439,7 +429,7 @@ bool dsm_input(uint16_t *values, uint16_t *num_values)
 
 	/*
 	 * Fetch bytes, but no more than we would need to complete
-	 * the current dsm_frame.
+	 * the current dsm frame.
 	 */
 	ret = read(dsm_fd, &dsm_frame[dsm_partial_frame_count], DSM_FRAME_SIZE - dsm_partial_frame_count);
 
@@ -450,18 +440,18 @@ bool dsm_input(uint16_t *values, uint16_t *num_values)
 	dsm_last_rx_time = now;
 
 	/*
-	 * Add bytes to the current dsm_frame
+	 * Add bytes to the current dsm frame
 	 */
 	dsm_partial_frame_count += ret;
 
 	/*
-	 * If we don't have a full dsm_frame, return
+	 * If we don't have a full dsm frame, return
 	 */
 	if (dsm_partial_frame_count < DSM_FRAME_SIZE)
 		return false;
 
 	/*
-	 * Great, it looks like we might have a dsm_frame.  Go ahead and
+	 * Great, it looks like we might have a dsm frame.  Go ahead and
 	 * decode it.
 	 */
 	dsm_partial_frame_count = 0;
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 83feeb9b61..57cffcc23a 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -184,7 +184,7 @@ extern void	controls_init(void);
 extern void	controls_tick(void);
 extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
-extern void     dsm_bind(uint16_t cmd, int pulses);
+extern void	dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
 
@@ -192,9 +192,9 @@ extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
 extern volatile uint8_t debug_level;
 
 /* send a debug message to the console */
-extern void isr_debug(uint8_t level, const char *fmt, ...);
+extern void	isr_debug(uint8_t level, const char *fmt, ...);
 
 #ifdef CONFIG_STM32_I2C1
-void i2c_dump(void);
-void i2c_reset(void);
+void		i2c_dump(void);
+void		i2c_reset(void);
 #endif

From 0301e2d145ae59e54030ce0081234bb22b76dba2 Mon Sep 17 00:00:00 2001
From: Jean Cyr <jcyr@dillobits.com>
Date: Fri, 16 Aug 2013 20:09:12 -0400
Subject: [PATCH 11/23] Flesh out PX4IO documentation comments and delete
 unnecessary class var

---
 src/drivers/px4io/px4io.cpp | 141 +++++++++++++++++++++++-------------
 1 file changed, 90 insertions(+), 51 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 960ac06ff4..c561ea83a0 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -89,21 +89,61 @@
 #define PX4IO_SET_DEBUG			_IOC(0xff00, 0)
 #define PX4IO_INAIR_RESTART_ENABLE	_IOC(0xff00, 1)
 
+/**
+ * The PX4IO class.
+ *
+ * Encapsulates PX4FMU to PX4IO communications modeled as file operations.
+ */
 class PX4IO : public device::I2C
 {
 public:
+	/**
+	 * Constructor.
+	 * 
+	 * Initialize all class variables.
+	 */
 	PX4IO();
+	/**
+	 * Destructor.
+	 * 
+	 * Wait for worker thread to terminate.
+	 */
 	virtual ~PX4IO();
 
+	/**
+	 * Initialize the PX4IO class.
+	 * 
+	 * Initialize the physical I2C interface to PX4IO. Retrieve relevant initial system parameters. Initialize PX4IO registers.
+	 */
 	virtual int		init();
 
+	/**
+	 * IO Control handler.
+	 * 
+	 * Handle all IOCTL calls to the PX4IO file descriptor.
+	 *
+	 * @param[in] filp file handle (not used). This function is always called directly through object reference
+	 * @param[in] cmd the IOCTL command
+	 * @param[in] the IOCTL command parameter (optional)
+	 */
 	virtual int		ioctl(file *filp, int cmd, unsigned long arg);
+
+	/**
+	 * write handler.
+	 * 
+	 * Handle writes to the PX4IO file descriptor.
+	 *
+	 * @param[in] filp file handle (not used). This function is always called directly through object reference
+	 * @param[in] buffer pointer to the data buffer to be written
+	 * @param[in] len size in bytes to be written
+	 * @return number of bytes written
+	 */
 	virtual ssize_t		write(file *filp, const char *buffer, size_t len);
 
 	/**
 	* Set the update rate for actuator outputs from FMU to IO.
 	*
-	* @param rate    The rate in Hz actuator outpus are sent to IO.
+	* @param[in] rate The rate in Hz actuator output are sent to IO.
 	*      Min 10 Hz, max 400 Hz
 	*/
 	int      		set_update_rate(int rate);
@@ -111,29 +151,41 @@ public:
 	/**
 	* Set the battery current scaling and bias
 	*
-	* @param amp_per_volt
-	* @param amp_bias
+	* @param[in] amp_per_volt
+	* @param[in] amp_bias
 	*/
 	void      		set_battery_current_scaling(float amp_per_volt, float amp_bias);
 
 	/**
 	 * Push failsafe values to IO.
 	 *
-	 * @param vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
-	 * @param len Number of channels, could up to 8
+	 * @param[in] vals Failsafe control inputs: in us PPM (900 for zero, 1500 for centered, 2100 for full)
+	 * @param[in] len Number of channels, could up to 8
 	 */
 	int			set_failsafe_values(const uint16_t *vals, unsigned len);
 
 	/**
-	* Print the current status of IO
-	*/
+	 * Print IO status.
+	 *
+	 * Print all relevant IO status information
+	 */
 	void			print_status();
 
+	/**
+	 * Set the DSM VCC is controlled by relay one flag
+	 *
+	 * @param[in] enable true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+	 */
 	inline void		set_dsm_vcc_ctl(bool enable)
 	{
 		_dsm_vcc_ctl = enable;
 	};
 
+	/**
+	 * Get the DSM VCC is controlled by relay one flag
+	 *
+	 * @return true=DSM satellite VCC is controlled by relay1, false=DSM satellite VCC not controlled
+	 */
 	inline bool		get_dsm_vcc_ctl()
 	{
 		return _dsm_vcc_ctl;
@@ -141,58 +193,48 @@ public:
 
 private:
 	// XXX
-	unsigned		_max_actuators;
-	unsigned		_max_controls;
-	unsigned		_max_rc_input;
-	unsigned		_max_relays;
-	unsigned		_max_transfer;
+	unsigned		_max_actuators;		///<Maximum # of actuators supported by PX4IO
+	unsigned		_max_controls;		///<Maximum # of controls supported by PX4IO
+	unsigned		_max_rc_input;		///<Maximum receiver channels supported by PX4IO
+	unsigned		_max_relays;		///<Maximum relays supported by PX4IO
+	unsigned		_max_transfer;		///<Maximum number of I2C transfers supported by PX4IO
 
-	unsigned 		_update_interval; ///< subscription interval limiting send rate
+	unsigned 		_update_interval;	///<Subscription interval limiting send rate
 
-	volatile int		_task;		///< worker task
-	volatile bool		_task_should_exit;
+	volatile int		_task;			///<worker task id
+	volatile bool		_task_should_exit;	///<worker terminate flag
 
-	int			_mavlink_fd;
+	int			_mavlink_fd;		///<mavlink file descriptor
 
-	perf_counter_t		_perf_update;
+	perf_counter_t		_perf_update;		///<local performance counter
 
 	/* cached IO state */
-	uint16_t		_status;
-	uint16_t		_alarms;
+	uint16_t		_status;		///<Various IO status flags
+	uint16_t		_alarms;		///<Various IO alarms
 
 	/* subscribed topics */
-	int			_t_actuators;	///< actuator controls topic
-	int			_t_armed;	///< system armed control topic
-	int 			_t_vstatus;	///< system / vehicle status
-	int			_t_param;	///< parameter update topic
+	int			_t_actuators;		///<actuator controls topic
+	int			_t_armed;		///<system armed control topic
+	int 			_t_vstatus;		///<system / vehicle status
+	int			_t_param;		///<parameter update topic
 
 	/* advertised topics */
-	orb_advert_t 		_to_input_rc;	///< rc inputs from io
-	orb_advert_t 		_to_actuators_effective; ///< effective actuator controls topic
-	orb_advert_t		_to_outputs;	///< mixed servo outputs topic
-	orb_advert_t		_to_battery;	///< battery status / voltage
+	orb_advert_t 		_to_input_rc;		///<rc inputs from IO topic
+	orb_advert_t 		_to_actuators_effective; ///<effective actuator controls topic
+	orb_advert_t		_to_outputs;		///<mixed servo outputs topic
+	orb_advert_t		_to_battery;		///<battery status / voltage topic
 
-	actuator_outputs_s	_outputs;	///< mixed outputs
-	actuator_controls_effective_s _controls_effective; ///< effective controls
+	actuator_outputs_s	_outputs;		///<mixed outputs
+	actuator_controls_effective_s _controls_effective; ///<effective controls
 
-	bool			_primary_pwm_device;	///< true if we are the default PWM output
+	bool			_primary_pwm_device;	///<true if we are the default PWM output
 
-	float			_battery_amp_per_volt;
-	float			_battery_amp_bias;
-	float			_battery_mamphour_total;
-	uint64_t		_battery_last_timestamp;
+	float			_battery_amp_per_volt;	///<current sensor amps/volt
+	float			_battery_amp_bias;	///<current sensor bias
+	float			_battery_mamphour_total;///<amp hours consumed so far
+	uint64_t		_battery_last_timestamp;///<last amp hour calculation timestamp
 
-	/**
-	 * Relay1 is dedicated to controlling DSM receiver power
-	 */
-
-	bool			_dsm_vcc_ctl;
-
-	/**
-	 * System armed
-	 */
-
-	bool			_system_armed;
+	bool			_dsm_vcc_ctl;		///<true if relay 1 controls DSM satellite RX power
 
 	/**
 	 * Trampoline to the worker task
@@ -361,8 +403,7 @@ PX4IO::PX4IO() :
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
 	_battery_last_timestamp(0),
-	_dsm_vcc_ctl(false),
-	_system_armed(false)
+	_dsm_vcc_ctl(false)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -687,7 +728,7 @@ PX4IO::task_main()
 				// See if bind parameter has been set, and reset it to 0
 				param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val);
 				if (dsm_bind_val) {
-					if (!_system_armed) {
+					if (!(_status & PX4IO_P_STATUS_FLAGS_ARMED)) {
 						if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) {
 							mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x');
 							ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7);
@@ -765,8 +806,6 @@ PX4IO::io_set_arming_state()
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-	_system_armed = vstatus.flag_system_armed;
-
 	if (armed.armed && !armed.lockdown) {
 		set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
 	} else {
@@ -1628,7 +1667,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 }
 
 ssize_t
-PX4IO::write(file *filp, const char *buffer, size_t len)
+PX4IO::write(file */*filp*/, const char *buffer, size_t len)
 {
 	unsigned count = len / 2;
 

From b71c0c1f491fc91561393c1ff1c1646b251fd96e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 17 Aug 2013 15:46:13 +0200
Subject: [PATCH 12/23] Added support for FMUv1 for RGB led and dim led support

---
 makefiles/config_px4fmu-v1_default.mk       | 1 +
 src/drivers/boards/px4fmu-v1/board_config.h | 2 ++
 src/drivers/drv_rgbled.h                    | 9 ++++++++-
 3 files changed, 11 insertions(+), 1 deletion(-)

diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index d7d9a1ecbd..8b4ea18bf5 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -30,6 +30,7 @@ MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
 MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
+MODULES		+= drivers/rgbled
 MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/md25
 MODULES		+= drivers/airspeed
diff --git a/src/drivers/boards/px4fmu-v1/board_config.h b/src/drivers/boards/px4fmu-v1/board_config.h
index 9d7c81f859..27621211a1 100644
--- a/src/drivers/boards/px4fmu-v1/board_config.h
+++ b/src/drivers/boards/px4fmu-v1/board_config.h
@@ -103,6 +103,7 @@ __BEGIN_DECLS
 #define PX4_I2C_BUS_ESC		1
 #define PX4_I2C_BUS_ONBOARD	2
 #define PX4_I2C_BUS_EXPANSION	3
+#define PX4_I2C_BUS_LED		3
 
 /*
  * Devices on the onboard bus.
@@ -112,6 +113,7 @@ __BEGIN_DECLS
 #define PX4_I2C_OBDEV_HMC5883	0x1e
 #define PX4_I2C_OBDEV_MS5611	0x76
 #define PX4_I2C_OBDEV_EEPROM	NOTDEFINED
+#define PX4_I2C_OBDEV_LED	0x55
 
 #define PX4_I2C_OBDEV_PX4IO_BL	0x18
 #define PX4_I2C_OBDEV_PX4IO	0x1a
diff --git a/src/drivers/drv_rgbled.h b/src/drivers/drv_rgbled.h
index 0f48f6f790..3c8bdec5d3 100644
--- a/src/drivers/drv_rgbled.h
+++ b/src/drivers/drv_rgbled.h
@@ -98,7 +98,14 @@ typedef enum {
 	RGBLED_COLOR_GREEN,
 	RGBLED_COLOR_BLUE,
 	RGBLED_COLOR_WHITE,
-	RGBLED_COLOR_AMBER
+	RGBLED_COLOR_AMBER,
+	RGBLED_COLOR_DIM_RED,
+	RGBLED_COLOR_DIM_YELLOW,
+	RGBLED_COLOR_DIM_PURPLE,
+	RGBLED_COLOR_DIM_GREEN,
+	RGBLED_COLOR_DIM_BLUE,
+	RGBLED_COLOR_DIM_WHITE,
+	RGBLED_COLOR_DIM_AMBER
 } rgbled_color_t;
 
 /* enum passed to RGBLED_SET_MODE ioctl()*/

From 64145252d4133b6df36229f548d02a692c3ec6fb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 17 Aug 2013 15:46:34 +0200
Subject: [PATCH 13/23] Added dim RGB implementation

---
 src/drivers/rgbled/rgbled.cpp | 21 +++++++++++++++++++++
 1 file changed, 21 insertions(+)

diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index 858be80587..f2543a33cc 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -294,6 +294,27 @@ RGBLED::set_color(rgbled_color_t color) {
 		case RGBLED_COLOR_AMBER:	// amber
 			set_rgb(255,20,0);
 			break;
+		case RGBLED_COLOR_DIM_RED:		// red
+			set_rgb(90,0,0);
+			break;
+		case RGBLED_COLOR_DIM_YELLOW:	// yellow
+			set_rgb(80,30,0);
+			break;
+		case RGBLED_COLOR_DIM_PURPLE:	// purple
+			set_rgb(45,0,45);
+			break;
+		case RGBLED_COLOR_DIM_GREEN:	// green
+			set_rgb(0,90,0);
+			break;
+		case RGBLED_COLOR_DIM_BLUE:		// blue
+			set_rgb(0,0,90);
+			break;
+		case RGBLED_COLOR_DIM_WHITE:	// white
+			set_rgb(30,30,30);
+			break;
+		case RGBLED_COLOR_DIM_AMBER:	// amber
+			set_rgb(80,20,0);
+			break;
 		default:
 			warnx("color unknown");
 			break;

From 6c45d9cb5cc4e9efb99aff84a1fe10f084a998c9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 17 Aug 2013 15:47:13 +0200
Subject: [PATCH 14/23] Fixed in-air timout, bumped protocol version

---
 src/drivers/px4io/px4io.cpp          | 2 +-
 src/modules/px4iofirmware/protocol.h | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 3aecd9b691..045b3ebcbb 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -508,7 +508,7 @@ PX4IO::init()
 			usleep(10000);
 
 			/* abort after 5s */
-			if ((hrt_absolute_time() - try_start_time)/1000 > 50000) {
+			if ((hrt_absolute_time() - try_start_time)/1000 > 3000) {
 				log("failed to recover from in-air restart (1), aborting IO driver init.");
 				return 1;
 			}
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 02df760685..97809d9c29 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -74,7 +74,7 @@
 #define REG_TO_FLOAT(_reg)	((float)REG_TO_SIGNED(_reg) / 10000.0f)
 #define FLOAT_TO_REG(_float)	SIGNED_TO_REG((int16_t)((_float) * 10000.0f))
 
-#define PX4IO_PROTOCOL_VERSION		2
+#define PX4IO_PROTOCOL_VERSION		3
 
 /* static configuration page */
 #define PX4IO_PAGE_CONFIG		0

From 6ff3f51f88c2335f225a2a391de5ee353487a69b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 17 Aug 2013 15:47:42 +0200
Subject: [PATCH 15/23] Added dim RGB led support, not operational yet

---
 src/modules/commander/commander.cpp | 35 ++++++++++++++++++++---------
 1 file changed, 25 insertions(+), 10 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 2e5d080b9f..30906034ea 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -52,6 +52,7 @@
 #include <errno.h>
 #include <debug.h>
 #include <sys/prctl.h>
+#include <sys/stat.h>
 #include <string.h>
 #include <math.h>
 #include <poll.h>
@@ -153,6 +154,8 @@ static unsigned int failsafe_lowlevel_timeout_ms;
 static unsigned int leds_counter;
 /* To remember when last notification was sent */
 static uint64_t last_print_mode_reject_time = 0;
+/* if connected via USB */
+static bool on_usb_power = false;
 
 
 /* tasks waiting for low prio thread */
@@ -205,6 +208,8 @@ transition_result_t check_main_state_machine(struct vehicle_status_s *current_st
 
 void print_reject_mode(const char *msg);
 
+void print_status();
+
 transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode);
 
 /**
@@ -244,6 +249,7 @@ int commander_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (thread_running) {
 			warnx("\tcommander is running\n");
+			print_status();
 
 		} else {
 			warnx("\tcommander not started\n");
@@ -265,6 +271,10 @@ void usage(const char *reason)
 	exit(1);
 }
 
+void print_status() {
+	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
+}
+
 void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
 {
 	/* result of the command */
@@ -865,9 +875,14 @@ int commander_thread_main(int argc, char *argv[])
 				status.load = 1.0f - ((float)interval_runtime / 1e6f);	//system load is time spent in non-idle
 
 			last_idle_time = system_load.tasks[0].total_runtime;
+
+			/* check if board is connected via USB */
+			struct stat statbuf;
+			//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
 		}
 
-		warnx("bat remaining: %2.2f", status.battery_remaining);
+		// XXX remove later
+		//warnx("bat remaining: %2.2f", status.battery_remaining);
 
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
 		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
@@ -1362,22 +1377,22 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 		if (armed->armed) {
 			/* armed, solid */
 			if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
-				pattern.color[0] = RGBLED_COLOR_AMBER;
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
 			} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
-				pattern.color[0] = RGBLED_COLOR_RED;
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
 			} else {
-				pattern.color[0] = RGBLED_COLOR_GREEN;
+				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN :RGBLED_COLOR_GREEN;
 			}
 			pattern.duration[0] = 1000;
 
 		} else if (armed->ready_to_arm) {
 			for (i=0; i<3; i++) {
 				if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
-					pattern.color[i*2] = RGBLED_COLOR_AMBER;
+					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
 				} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
-					pattern.color[i*2] = RGBLED_COLOR_RED;
+					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
 				} else {
-					pattern.color[i*2] = RGBLED_COLOR_GREEN;
+					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
 				}
 				pattern.duration[i*2] = 200;
 
@@ -1385,13 +1400,13 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 				pattern.duration[i*2+1] = 800;
 			}
 			if (status->condition_global_position_valid) {
-				pattern.color[i*2] = RGBLED_COLOR_BLUE;
+				pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
 				pattern.duration[i*2] = 1000;
 				pattern.color[i*2+1] = RGBLED_COLOR_OFF;
 				pattern.duration[i*2+1] = 800;
 			} else {
 				for (i=3; i<6; i++) {
-					pattern.color[i*2] = RGBLED_COLOR_BLUE;
+					pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
 					pattern.duration[i*2] = 100;
 					pattern.color[i*2+1] = RGBLED_COLOR_OFF;
 					pattern.duration[i*2+1] = 100;
@@ -1402,7 +1417,7 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 
 		} else {
 			for (i=0; i<3; i++) {
-				pattern.color[i*2] = RGBLED_COLOR_RED;
+				pattern.color[i*2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
 				pattern.duration[i*2] = 200;
 				pattern.color[i*2+1] = RGBLED_COLOR_OFF;
 				pattern.duration[i*2+1] = 200;

From e9b6cfd671c72b75f2bf79bf1031ea8697f430b2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 17 Aug 2013 15:48:13 +0200
Subject: [PATCH 16/23] Fixed startup order of F330 script

---
 ROMFS/px4fmu_common/init.d/10_io_f330 | 18 +++++-------------
 ROMFS/px4fmu_common/init.d/rcS        | 13 +++++++++++++
 2 files changed, 18 insertions(+), 13 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330
index b3fb027573..0634d650e0 100644
--- a/ROMFS/px4fmu_common/init.d/10_io_f330
+++ b/ROMFS/px4fmu_common/init.d/10_io_f330
@@ -44,6 +44,11 @@ param set MAV_TYPE 2
 #
 mavlink start
 usleep 5000
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
  
 #
 # Start PX4IO interface (depends on orb, commander)
@@ -77,11 +82,6 @@ mixer load /dev/pwm_output /etc/mixers/FMU_quad_x.mix
 # Start the sensors (depends on orb, px4io)
 #
 sh /etc/init.d/rc.sensors
-
-#
-# Start the commander (depends on orb, mavlink)
-#
-commander start
  
 #
 # Start GPS interface (depends on orb)
@@ -102,15 +102,7 @@ if [ $BOARD == fmuv1 ]
 then
 	px4io limit 200
 	sdlog2 start -r 50 -a -b 16
-	if blinkm start
-	then
-		blinkm systemstate
-	fi
 else
 	px4io limit 400
 	sdlog2 start -r 200 -a -b 16
-	if rgbled start
-	then
-		#rgbled systemstate
-	fi
 fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index f0ee1a0c6e..60cc13611b 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -81,6 +81,19 @@ else
 	fi
 fi
 
+#
+# Start system state indicator
+#
+if rgbled start
+then
+	echo "Using external RGB Led"
+else
+	if blinkm start
+	then
+		blinkm systemstate
+	fi
+fi
+
 #
 # Check if PX4IO Firmware should be upgraded (from Andrew Tridgell)
 #

From 3f800e586112ae62bc4082ad616191206b490cb3 Mon Sep 17 00:00:00 2001
From: Jean Cyr <jcyr@dillobits.com>
Date: Sat, 17 Aug 2013 10:29:35 -0400
Subject: [PATCH 17/23] Make it obvious that file * isn't used here

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

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index c561ea83a0..fed83ea1ac 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1667,7 +1667,8 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 }
 
 ssize_t
-PX4IO::write(file */*filp*/, const char *buffer, size_t len)
+PX4IO::write(file * /*filp*/, const char *buffer, size_t len)
+/* Make it obvious that file * isn't used here */
 {
 	unsigned count = len / 2;
 

From 74802f0692ad61ef3995b2f05c7af043ab9fcaf3 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 17 Aug 2013 18:01:51 +0200
Subject: [PATCH 18/23] Added possibility to set board orientation

---
 src/modules/sensors/sensor_params.c |   3 +
 src/modules/sensors/sensors.cpp     | 148 ++++++++++++++++++++++++++--
 2 files changed, 141 insertions(+), 10 deletions(-)

diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 255dfed184..8d39929636 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -70,6 +70,9 @@ PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
 PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0);
 
+PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
+PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
+
 PARAM_DEFINE_FLOAT(RC1_MIN, 1000.0f);
 PARAM_DEFINE_FLOAT(RC1_TRIM, 1500.0f);
 PARAM_DEFINE_FLOAT(RC1_MAX, 2000.0f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 42268b9715..7299b21bc7 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -50,6 +50,7 @@
 #include <stdio.h>
 #include <errno.h>
 #include <math.h>
+#include <mathlib/mathlib.h>
 
 #include <nuttx/analog/adc.h>
 
@@ -137,6 +138,77 @@
 
 #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
 
+/**
+ * Enum for board and external compass rotations
+ * copied from https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Math/rotations.h
+ */
+enum Rotation {
+    ROTATION_NONE                = 0,
+    ROTATION_YAW_45              = 1,
+    ROTATION_YAW_90              = 2,
+    ROTATION_YAW_135             = 3,
+    ROTATION_YAW_180             = 4,
+    ROTATION_YAW_225             = 5,
+    ROTATION_YAW_270             = 6,
+    ROTATION_YAW_315             = 7,
+    ROTATION_ROLL_180            = 8,
+    ROTATION_ROLL_180_YAW_45     = 9,
+    ROTATION_ROLL_180_YAW_90     = 10,
+    ROTATION_ROLL_180_YAW_135    = 11,
+    ROTATION_PITCH_180           = 12,
+    ROTATION_ROLL_180_YAW_225    = 13,
+    ROTATION_ROLL_180_YAW_270    = 14,
+    ROTATION_ROLL_180_YAW_315    = 15,
+    ROTATION_ROLL_90             = 16,
+    ROTATION_ROLL_90_YAW_45      = 17,
+    ROTATION_ROLL_90_YAW_90      = 18,
+    ROTATION_ROLL_90_YAW_135     = 19,
+    ROTATION_ROLL_270            = 20,
+    ROTATION_ROLL_270_YAW_45     = 21,
+    ROTATION_ROLL_270_YAW_90     = 22,
+    ROTATION_ROLL_270_YAW_135    = 23,
+    ROTATION_PITCH_90            = 24,
+    ROTATION_PITCH_270           = 25,
+    ROTATION_MAX
+};
+
+typedef struct
+{
+    uint16_t roll;
+    uint16_t pitch;
+    uint16_t yaw;
+} rot_lookup_t;
+
+const rot_lookup_t rot_lookup[] =
+{
+    {  0,   0,   0 },
+    {  0,   0,  45 },
+    {  0,   0,  90 },
+    {  0,   0, 135 },
+    {  0,   0, 180 },
+    {  0,   0, 225 },
+    {  0,   0, 270 },
+    {  0,   0, 315 },
+    {180,   0,   0 },
+    {180,   0,  45 },
+    {180,   0,  90 },
+    {180,   0, 135 },
+    {  0, 180,   0 },
+    {180,   0, 225 },
+    {180,   0, 270 },
+    {180,   0, 315 },
+    { 90,   0,   0 },
+    { 90,   0,  45 },
+    { 90,   0,  90 },
+    { 90,   0, 135 },
+    {270,   0,   0 },
+    {270,   0,  45 },
+    {270,   0,  90 },
+    {270,   0, 135 },
+    {  0,  90,   0 },
+    {  0, 270,   0 }
+};
+
 /**
  * Sensor app start / stop handling function
  *
@@ -210,6 +282,9 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 
+	math::Matrix	_board_rotation;			/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix	_external_mag_rotation;	/**< rotation matrix for the orientation that an external mag is mounted */
+
 	struct {
 		float min[_rc_max_chan_count];
 		float trim[_rc_max_chan_count];
@@ -227,6 +302,9 @@ private:
 		float accel_scale[3];
 		float diff_pres_offset_pa;
 
+		int board_rotation;
+		int external_mag_rotation;
+
 		int rc_type;
 
 		int rc_map_roll;
@@ -306,6 +384,9 @@ private:
 
 		param_t battery_voltage_scaling;
 
+		param_t board_rotation;
+		param_t external_mag_rotation;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -314,6 +395,11 @@ private:
 	 */
 	int		parameters_update();
 
+	/**
+	 * Get the rotation matrices
+	 */
+	void		get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+
 	/**
 	 * Do accel-related initialisation.
 	 */
@@ -450,7 +536,10 @@ Sensors::Sensors() :
 	_diff_pres_pub(-1),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "sensor task update")),
+
+	_board_rotation(3,3),
+	_external_mag_rotation(3,3)
 {
 
 	/* basic r/c parameters */
@@ -540,6 +629,10 @@ Sensors::Sensors() :
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
+	/* rotations */
+	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -731,9 +824,33 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}
 
+	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+	param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
+
+	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
+	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
+
 	return OK;
 }
 
+void
+Sensors::get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
+{
+	/* first set to zero */
+	rot_matrix->Matrix::zero(3,3);
+
+	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
+	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
+	float yaw   = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
+
+	math::EulerAngles euler(roll, pitch, yaw);
+
+	math::Dcm R(euler);
+
+	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+		(*rot_matrix)(i,j) = R(i, j);
+}
+
 void
 Sensors::accel_init()
 {
@@ -874,9 +991,12 @@ Sensors::accel_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report);
 
-		raw.accelerometer_m_s2[0] = accel_report.x;
-		raw.accelerometer_m_s2[1] = accel_report.y;
-		raw.accelerometer_m_s2[2] = accel_report.z;
+		math::Vector3 vect = {accel_report.x, accel_report.y, accel_report.z};
+		vect = _board_rotation*vect;
+
+		raw.accelerometer_m_s2[0] = vect(0);
+		raw.accelerometer_m_s2[1] = vect(1);
+		raw.accelerometer_m_s2[2] = vect(2);
 
 		raw.accelerometer_raw[0] = accel_report.x_raw;
 		raw.accelerometer_raw[1] = accel_report.y_raw;
@@ -897,9 +1017,12 @@ Sensors::gyro_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report);
 
-		raw.gyro_rad_s[0] = gyro_report.x;
-		raw.gyro_rad_s[1] = gyro_report.y;
-		raw.gyro_rad_s[2] = gyro_report.z;
+		math::Vector3 vect = {gyro_report.x, gyro_report.y, gyro_report.z};
+		vect = _board_rotation*vect;
+
+		raw.gyro_rad_s[0] = vect(0);
+		raw.gyro_rad_s[1] = vect(1);
+		raw.gyro_rad_s[2] = vect(2);
 
 		raw.gyro_raw[0] = gyro_report.x_raw;
 		raw.gyro_raw[1] = gyro_report.y_raw;
@@ -920,9 +1043,14 @@ Sensors::mag_poll(struct sensor_combined_s &raw)
 
 		orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report);
 
-		raw.magnetometer_ga[0] = mag_report.x;
-		raw.magnetometer_ga[1] = mag_report.y;
-		raw.magnetometer_ga[2] = mag_report.z;
+		// XXX TODO add support for external mag orientation
+
+		math::Vector3 vect = {mag_report.x, mag_report.y, mag_report.z};
+		vect = _board_rotation*vect;
+
+		raw.magnetometer_ga[0] = vect(0);
+		raw.magnetometer_ga[1] = vect(1);
+		raw.magnetometer_ga[2] = vect(2);
 
 		raw.magnetometer_raw[0] = mag_report.x_raw;
 		raw.magnetometer_raw[1] = mag_report.y_raw;

From eda528157a04185cbb1342c152c4ac715f67771c Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 17 Aug 2013 18:40:28 +0200
Subject: [PATCH 19/23] Make state updates atomic (just to be really, really
 sure)

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

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index ef3890b713..5842a33b19 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -69,6 +69,11 @@ static bool navigation_state_changed = true;
 transition_result_t
 arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed)
 {
+	/*
+	 * Perform an atomic state update
+	 */
+	irqstate_t flags = irqsave();
+
 	transition_result_t ret = TRANSITION_DENIED;
 
 	/* only check transition if the new state is actually different from the current one */
@@ -168,11 +173,15 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 		if (ret == TRANSITION_CHANGED) {
 			status->arming_state = new_arming_state;
 			arming_state_changed = true;
-		} else {
-			warnx("arming transition rejected");
 		}
 	}
 
+	/* end of atomic state update */
+	irqrestore(flags);
+
+	if (ret == TRANSITION_DENIED)
+		warnx("arming transition rejected");
+
 	return ret;
 }
 

From 628e806df5b05ea8a44d46f29606db03fee1fce9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 17 Aug 2013 18:48:14 +0200
Subject: [PATCH 20/23] Minor style changes

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

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 7299b21bc7..c47f6bb7d9 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -139,8 +139,8 @@
 #define limit_minus_one_to_one(arg) (arg < -1.0f) ? -1.0f : ((arg > 1.0f) ? 1.0f : arg)
 
 /**
- * Enum for board and external compass rotations
- * copied from https://github.com/diydrones/ardupilot/blob/master/libraries/AP_Math/rotations.h
+ * Enum for board and external compass rotations.
+ * This enum maps from board attitude to airframe attitude.
  */
 enum Rotation {
     ROTATION_NONE                = 0,
@@ -261,8 +261,8 @@ private:
 	int		_mag_sub;			/**< raw mag data subscription */
 	int 		_rc_sub;			/**< raw rc channels data subscription */
 	int		_baro_sub;			/**< raw baro data subscription */
-	int     _airspeed_sub;		/**< airspeed subscription */
-	int		_diff_pres_sub;		/**< raw differential pressure subscription */
+	int		_airspeed_sub;			/**< airspeed subscription */
+	int		_diff_pres_sub;			/**< raw differential pressure subscription */
 	int		_vstatus_sub;			/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;			/**< notification of manual control updates */
@@ -282,8 +282,8 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;
 
-	math::Matrix	_board_rotation;			/**< rotation matrix for the orientation that the board is mounted */
-	math::Matrix	_external_mag_rotation;	/**< rotation matrix for the orientation that an external mag is mounted */
+	math::Matrix	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 
 	struct {
 		float min[_rc_max_chan_count];
@@ -291,7 +291,6 @@ private:
 		float max[_rc_max_chan_count];
 		float rev[_rc_max_chan_count];
 		float dz[_rc_max_chan_count];
-		// float ex[_rc_max_chan_count];
 		float scaling_factor[_rc_max_chan_count];
 
 		float gyro_offset[3];
@@ -343,7 +342,6 @@ private:
 		param_t max[_rc_max_chan_count];
 		param_t rev[_rc_max_chan_count];
 		param_t dz[_rc_max_chan_count];
-		// param_t ex[_rc_max_chan_count];
 		param_t rc_type;
 
 		param_t rc_demix;
@@ -874,7 +872,7 @@ Sensors::accel_init()
 		/* set the driver to poll at 1000Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 1000);
 
-		#else
+		#elif CONFIG_ARCH_BOARD_PX4FMU_V2
 
 		/* set the accel internal sampling rate up to at leat 800Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -882,6 +880,9 @@ Sensors::accel_init()
 		/* set the driver to poll at 800Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 800);
 
+		#else
+			#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+
 		#endif
 
 		warnx("using system accel");

From b08ca02410cc02f75bbfe154963edcea6767972e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 17 Aug 2013 19:55:40 +0200
Subject: [PATCH 21/23] Hotfix for UART5

---
 nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index 5b91930c91..ae941804ec 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -268,7 +268,7 @@ CONFIG_USART2_RXDMA=y
 # CONFIG_USART3_RXDMA is not set
 # CONFIG_UART4_RXDMA is not set
 # CONFIG_UART5_RS485 is not set
-CONFIG_UART5_RXDMA=y
+CONFIG_UART5_RXDMA=n
 # CONFIG_USART6_RS485 is not set
 CONFIG_USART6_RXDMA=y
 # CONFIG_USART7_RXDMA is not set

From e5f1a7c2c3a67648829ec0dff5bf290dddc25847 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 18 Aug 2013 12:42:24 +0200
Subject: [PATCH 22/23] Better transparency in IO mode display, fixes to
 commander arming, minimum publishing rate for arming

---
 src/drivers/px4io/px4io.cpp                   |  4 +-
 src/modules/commander/commander.cpp           | 51 +++++++++++++++++--
 .../commander/state_machine_helper.cpp        |  2 +-
 3 files changed, 49 insertions(+), 8 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 8318238e2e..0f90db858a 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1519,8 +1519,8 @@ PX4IO::print_status()
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
 	printf("arming 0x%04x%s%s%s%s%s%s\n",
 		arming,
-		((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)          ? " FMU_ARMED" : ""),
-		((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)	    ? " IO_ARM_OK" : ""),
+		((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"),
 		((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)   ? " FAILSAFE_CUSTOM" : ""),
 		((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)   ? " INAIR_RESTART_OK" : ""),
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 30906034ea..12543800e3 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -273,6 +273,43 @@ void usage(const char *reason)
 
 void print_status() {
 	warnx("usb powered: %s", (on_usb_power) ? "yes" : "no");
+
+	/* read all relevant states */
+	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
+	struct vehicle_status_s state;
+	orb_copy(ORB_ID(vehicle_status), state_sub, &state);
+
+	const char* armed_str;
+
+	switch (state.arming_state) {
+		case ARMING_STATE_INIT:
+			armed_str = "INIT";
+			break;
+		case ARMING_STATE_STANDBY:
+			armed_str = "STANDBY";
+			break;
+		case ARMING_STATE_ARMED:
+			armed_str = "ARMED";
+			break;
+		case ARMING_STATE_ARMED_ERROR:
+			armed_str = "ARMED_ERROR";
+			break;
+		case ARMING_STATE_STANDBY_ERROR:
+			armed_str = "STANDBY_ERROR";
+			break;
+		case ARMING_STATE_REBOOT:
+			armed_str = "REBOOT";
+			break;
+		case ARMING_STATE_IN_AIR_RESTORE:
+			armed_str = "IN_AIR_RESTORE";
+			break;
+		default:
+			armed_str = "ERR: UNKNOWN STATE";
+			break;
+	}
+
+
+	warnx("arming: %s", armed_str);
 }
 
 void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed)
@@ -945,9 +982,9 @@ int commander_thread_main(int argc, char *argv[])
 
 		/* store current state to reason later about a state change */
 		// bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok;
-		bool global_pos_valid = status.condition_global_position_valid;
-		bool local_pos_valid = status.condition_local_position_valid;
-		bool airspeed_valid = status.condition_airspeed_valid;
+		// bool global_pos_valid = status.condition_global_position_valid;
+		// bool local_pos_valid = status.condition_local_position_valid;
+		// bool airspeed_valid = status.condition_airspeed_valid;
 
 
 		/* check for global or local position updates, set a timeout of 2s */
@@ -1274,11 +1311,15 @@ int commander_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
 		}
 
-		/* publish vehicle status at least with 1 Hz */
-		if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
+		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
+		if (counter % (200000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) {
 			status.counter++;
 			status.timestamp = t;
 			orb_publish(ORB_ID(vehicle_status), status_pub, &status);
+			control_mode.timestamp = t;
+			orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode);
+			armed.timestamp = t;
+			orb_publish(ORB_ID(actuator_armed), armed_pub, &armed);
 		}
 
 		/* play arming and battery warning tunes */
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 5842a33b19..1e31573d6b 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -119,7 +119,7 @@ arming_state_transition(struct vehicle_status_s *status, const struct safety_s *
 			{
 				ret = TRANSITION_CHANGED;
 				armed->armed = true;
-				armed->ready_to_arm = false;
+				armed->ready_to_arm = true;
 			}
 
 			break;

From 57c5240f0283e2f3e325287f46ec87bd790122d4 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 18 Aug 2013 13:57:21 +0200
Subject: [PATCH 23/23] Make a distinctive sound when the IO start fails (e.g.
 due to version mismatch)

---
 ROMFS/px4fmu_common/init.d/10_io_f330 | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/10_io_f330 b/ROMFS/px4fmu_common/init.d/10_io_f330
index 0634d650e0..48636292c2 100644
--- a/ROMFS/px4fmu_common/init.d/10_io_f330
+++ b/ROMFS/px4fmu_common/init.d/10_io_f330
@@ -53,8 +53,13 @@ commander start
 #
 # Start PX4IO interface (depends on orb, commander)
 #
-px4io start
-pwm -u 400 -m 0xff
+if px4io start
+then
+	pwm -u 400 -m 0xff
+else
+	# SOS
+	tone_alarm 6
+fi
  
 #
 # Allow PX4IO to recover from midair restarts.