From 114b7b696d54d031c899db4ffb91c125204fbba2 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Mon, 14 Oct 2013 11:14:56 +0200
Subject: [PATCH 01/47] sdlog2: VER message added instead of FWRV

---
 src/modules/sdlog2/sdlog2.c          | 35 ++++++++++++++--
 src/modules/sdlog2/sdlog2_messages.h | 24 +++--------
 src/modules/sdlog2/sdlog2_version.h  | 62 ++++++++++++++++++++++++++++
 3 files changed, 99 insertions(+), 22 deletions(-)
 create mode 100644 src/modules/sdlog2/sdlog2_version.h

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index ec8033202d..edb21c7ab2 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -90,6 +90,7 @@
 #include "logbuffer.h"
 #include "sdlog2_format.h"
 #include "sdlog2_messages.h"
+#include "sdlog2_version.h"
 
 #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
 		log_msgs_written++; \
@@ -182,6 +183,10 @@ static void sdlog2_stop_log(void);
  */
 static void write_formats(int fd);
 
+/**
+ * Write version message to log file.
+ */
+static void write_version(int fd);
 
 static bool file_exist(const char *filename);
 
@@ -359,6 +364,9 @@ static void *logwriter_thread(void *arg)
 	/* write log messages formats */
 	write_formats(log_file);
 
+	/* write version */
+	write_version(log_file);
+
 	int poll_count = 0;
 
 	void *read_ptr;
@@ -487,14 +495,13 @@ void sdlog2_stop_log()
 	sdlog2_status();
 }
 
-
 void write_formats(int fd)
 {
 	/* construct message format packet */
 	struct {
 		LOG_PACKET_HEADER;
 		struct log_format_s body;
-	} log_format_packet = {
+	} log_msg_format = {
 		LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG),
 	};
 
@@ -502,13 +509,33 @@ void write_formats(int fd)
 	int i;
 
 	for (i = 0; i < log_formats_num; i++) {
-		log_format_packet.body = log_formats[i];
-		log_bytes_written += write(fd, &log_format_packet, sizeof(log_format_packet));
+		log_msg_format.body = log_formats[i];
+		log_bytes_written += write(fd, &log_msg_format, sizeof(log_msg_format));
 	}
 
 	fsync(fd);
 }
 
+void write_version(int fd)
+{
+	/* construct version message */
+	struct {
+		LOG_PACKET_HEADER;
+		struct log_VER_s body;
+	} log_msg_VER = {
+		LOG_PACKET_HEADER_INIT(127),
+	};
+
+	/* fill message format packet for each format and write to log */
+	int i;
+
+	strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git));
+	strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch));
+	log_bytes_written += write(fd, &log_msg_VER, sizeof(log_msg_VER));
+
+	fsync(fd);
+}
+
 int sdlog2_thread_main(int argc, char *argv[])
 {
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 0e88da0547..9f709e4591 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -253,28 +253,16 @@ struct log_GVSP_s {
 	float vz;
 };
 
-/* --- FWRV - FIRMWARE REVISION --- */
-#define LOG_FWRV_MSG 20
-struct log_FWRV_s {
-	char    fw_revision[64];
+/* --- VER - VERSION --- */
+#define LOG_VER_MSG 127
+struct log_VER_s {
+	char arch[16];
+	char fw_git[64];
 };
 
 #pragma pack(pop)
 
-
-/*
- GIT_VERSION is defined at build time via a Makefile call to the
- git command line. We create a fake log message format for 
- the firmware revision "FWRV" that is written to every log
- header. This makes it easier to determine which version
- of the firmware was running when a log was created.
- */
-#define FREEZE_STR(s) #s
-#define STRINGIFY(s) FREEZE_STR(s)
-#define FW_VERSION_STR  STRINGIFY(GIT_VERSION)
-
 /* construct list of all message formats */
-
 static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(TIME, "Q", "StartTime"),
 	LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
@@ -295,7 +283,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
-	LOG_FORMAT(FWRV,"Z",FW_VERSION_STR),
+	LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
diff --git a/src/modules/sdlog2/sdlog2_version.h b/src/modules/sdlog2/sdlog2_version.h
new file mode 100644
index 0000000000..c6a9ba6387
--- /dev/null
+++ b/src/modules/sdlog2/sdlog2_version.h
@@ -0,0 +1,62 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *
+ * 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 sdlog2_version.h
+ *
+ * Tools for system version detection.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef SDLOG2_VERSION_H_
+#define SDLOG2_VERSION_H_
+
+/*
+ GIT_VERSION is defined at build time via a Makefile call to the
+ git command line.
+ */
+#define FREEZE_STR(s) #s
+#define STRINGIFY(s) FREEZE_STR(s)
+#define FW_GIT STRINGIFY(GIT_VERSION)
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
+#define	HW_ARCH "PX4FMU_V1"
+#endif
+
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V2
+#define	HW_ARCH "PX4FMU_V2"
+#endif
+
+#endif /* SDLOG2_VERSION_H_ */

From 3dcd5dbd0e25432049e5ba6971851931764ae043 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Oct 2013 08:39:57 +0200
Subject: [PATCH 02/47] Piping through manual control channels

---
 .../fw_att_control/fw_att_control_main.cpp    |  4 ++++
 .../multirotor_att_control_main.c             |  6 +++++
 src/modules/sensors/sensors.cpp               | 22 +++++++++++++++++++
 3 files changed, 32 insertions(+)

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index ae9aaa2da6..eb67874db4 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -683,6 +683,10 @@ FixedwingAttitudeControl::task_main()
 				_actuators.control[4] = _manual.flaps;
 			}
 
+			_actuators.control[5] = _manual.aux1;
+			_actuators.control[6] = _manual.aux2;
+			_actuators.control[7] = _manual.aux3;
+
 			/* lazily publish the setpoint only once available */
 			_actuators.timestamp = hrt_absolute_time();
 
diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index 44f8f664bc..60a211817c 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -368,6 +368,12 @@ mc_thread_main(int argc, char *argv[])
 				actuators.control[3] = 0.0f;
 			}
 
+			/* fill in manual control values */
+			actuators.control[4] = manual.flaps;
+			actuators.control[5] = manual.aux1;
+			actuators.control[6] = manual.aux2;
+			actuators.control[7] = manual.aux3;
+
 			actuators.timestamp = hrt_absolute_time();
 			orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators);
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 3fcacaf8f9..630131e1e8 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -74,6 +74,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/rc_channels.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/battery_status.h>
@@ -264,6 +265,7 @@ private:
 
 	orb_advert_t	_sensor_pub;			/**< combined sensor data topic */
 	orb_advert_t	_manual_control_pub;		/**< manual control signal topic */
+	orb_advert_t	_actuator_group_3_pub;		/**< manual control as actuator topic */
 	orb_advert_t	_rc_pub;			/**< raw r/c control topic */
 	orb_advert_t	_battery_pub;			/**< battery status */
 	orb_advert_t	_airspeed_pub;			/**< airspeed */
@@ -519,6 +521,7 @@ Sensors::Sensors() :
 /* publications */
 	_sensor_pub(-1),
 	_manual_control_pub(-1),
+	_actuator_group_3_pub(-1),
 	_rc_pub(-1),
 	_battery_pub(-1),
 	_airspeed_pub(-1),
@@ -1318,6 +1321,7 @@ Sensors::rc_poll()
 		orb_copy(ORB_ID(input_rc), _rc_sub, &rc_input);
 
 		struct manual_control_setpoint_s manual_control;
+		struct actuator_controls_s actuator_group_3;
 
 		/* initialize to default values */
 		manual_control.roll = NAN;
@@ -1485,6 +1489,16 @@ Sensors::rc_poll()
 			manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled);
 		}
 
+		/* copy from mapped manual control to control group 3 */
+		actuator_group_3.control[0] = manual_control.roll;
+		actuator_group_3.control[1] = manual_control.pitch;
+		actuator_group_3.control[2] = manual_control.yaw;
+		actuator_group_3.control[3] = manual_control.throttle;
+		actuator_group_3.control[4] = manual_control.flaps;
+		actuator_group_3.control[5] = manual_control.aux1;
+		actuator_group_3.control[6] = manual_control.aux2;
+		actuator_group_3.control[7] = manual_control.aux3;
+
 		/* check if ready for publishing */
 		if (_rc_pub > 0) {
 			orb_publish(ORB_ID(rc_channels), _rc_pub, &_rc);
@@ -1501,6 +1515,14 @@ Sensors::rc_poll()
 		} else {
 			_manual_control_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual_control);
 		}
+
+		/* check if ready for publishing */
+		if (_actuator_group_3_pub > 0) {
+			orb_publish(ORB_ID(actuator_controls_3), _actuator_group_3_pub, &actuator_group_3);
+
+		} else {
+			_actuator_group_3_pub = orb_advertise(ORB_ID(actuator_controls_3), &actuator_group_3);
+		}
 	}
 
 }

From 99068e864beaabf3b2dfabfb2e2f1c6cb7df7095 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Oct 2013 09:10:40 +0200
Subject: [PATCH 03/47] Enable payload channels as direct pass-through from
 manual control

---
 ROMFS/px4fmu_common/init.d/rcS            |  6 +++---
 ROMFS/px4fmu_common/mixers/FMU_AERT.mix   | 20 ++++++++++++++++++++
 ROMFS/px4fmu_common/mixers/FMU_AET.mix    | 20 ++++++++++++++++++++
 ROMFS/px4fmu_common/mixers/FMU_Q.mix      | 18 ++++++++++++++++++
 ROMFS/px4fmu_common/mixers/FMU_RET.mix    | 20 ++++++++++++++++++++
 ROMFS/px4fmu_common/mixers/FMU_X5.mix     | 19 +++++++++++++++++++
 ROMFS/px4fmu_common/mixers/FMU_delta.mix  | 20 ++++++++++++++++++++
 ROMFS/px4fmu_common/mixers/FMU_hex_+.mix  | 11 +++++++++++
 ROMFS/px4fmu_common/mixers/FMU_hex_x.mix  | 11 +++++++++++
 ROMFS/px4fmu_common/mixers/FMU_quad_+.mix | 20 ++++++++++++++++++++
 ROMFS/px4fmu_common/mixers/FMU_quad_v.mix | 19 +++++++++++++++++++
 ROMFS/px4fmu_common/mixers/FMU_quad_w.mix | 19 +++++++++++++++++++
 ROMFS/px4fmu_common/mixers/FMU_quad_x.mix | 19 +++++++++++++++++++
 src/modules/sensors/sensor_params.c       |  5 +++--
 14 files changed, 222 insertions(+), 5 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 93e76184d4..e0813b0319 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -166,11 +166,11 @@ then
 		set MODE custom
 	fi
 	
-	if param compare SYS_AUTOSTART 10
-	then
+	#if param compare SYS_AUTOSTART 10
+	#then
 		sh /etc/init.d/10_dji_f330
 		set MODE custom
-	fi
+	#fi
 
 	if param compare SYS_AUTOSTART 11
 	then
diff --git a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
index 75e82bb00a..0ec663e35e 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_AERT.mix
@@ -62,3 +62,23 @@ range.  Inputs below zero are treated as zero.
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
+
+
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_AET.mix b/ROMFS/px4fmu_common/mixers/FMU_AET.mix
index 20cb88b916..c73cb2a62d 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_AET.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_AET.mix
@@ -58,3 +58,23 @@ range.  Inputs below zero are treated as zero.
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
+
+
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_Q.mix b/ROMFS/px4fmu_common/mixers/FMU_Q.mix
index ebcb66b248..17ff711513 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_Q.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_Q.mix
@@ -50,3 +50,21 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_RET.mix b/ROMFS/px4fmu_common/mixers/FMU_RET.mix
index 95beb8927c..f07c34ac8a 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_RET.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_RET.mix
@@ -51,3 +51,23 @@ range.  Inputs below zero are treated as zero.
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
+
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_X5.mix b/ROMFS/px4fmu_common/mixers/FMU_X5.mix
index 9f81e1dc3a..6108683549 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_X5.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_X5.mix
@@ -48,3 +48,22 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
+
diff --git a/ROMFS/px4fmu_common/mixers/FMU_delta.mix b/ROMFS/px4fmu_common/mixers/FMU_delta.mix
index 9814667041..f0aa6650d9 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_delta.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_delta.mix
@@ -48,3 +48,23 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
 
+
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
+
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
index b5e38ce9ef..f8f9f0e4dc 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix
@@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the + configuration. All co
 are mixed 100%.
 
 R: 6+ 10000 10000 10000 0
+
+Gimbal / payload mixer for last two channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
index 8e8d122adc..26b40b9e95 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix
@@ -5,3 +5,14 @@ This file defines a single mixer for a hexacopter in the X configuration.  All c
 are mixed 100%.
 
 R: 6x 10000 10000 10000 0
+
+Gimbal / payload mixer for last two channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix
index dfdf1d58e0..cd9a9cfab6 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_+.mix
@@ -5,3 +5,23 @@ This file defines a single mixer for a quadrotor in the + configuration. All con
 are mixed 100%.
 
 R: 4+ 10000 10000 10000 0
+
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix
index 2a4a0f3419..520aba6359 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_v.mix
@@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the V configuration.  All co
 are mixed 100%.
 
 R: 4v 10000 10000 10000 0
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
index 81b4af30b2..58e6af74ba 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_w.mix
@@ -4,3 +4,22 @@ Multirotor mixer for PX4FMU
 This file defines a single mixer for a quadrotor with a wide configuration.  All controls are mixed 100%.
 
 R: 4w 10000 10000 10000 0
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix
index 12a3bee20c..fa21c80128 100644
--- a/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_quad_x.mix
@@ -5,3 +5,22 @@ This file defines a single mixer for a quadrotor in the X configuration.  All co
 are mixed 100%.
 
 R: 4x 10000 10000 10000 0
+
+Gimbal / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 4d719e0e15..682cb3d81c 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -193,8 +193,9 @@ PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
 
 PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0);
 
-PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);	/**< default function: camera yaw / azimuth */
-PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera pitch / tilt */
+PARAM_DEFINE_INT32(RC_MAP_AUX1, 0);	/**< default function: camera pitch */
+PARAM_DEFINE_INT32(RC_MAP_AUX2, 0);	/**< default function: camera roll */
+PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);	/**< default function: camera azimuth / yaw */
 
 PARAM_DEFINE_FLOAT(RC_SCALE_ROLL, 0.6f);
 PARAM_DEFINE_FLOAT(RC_SCALE_PITCH, 0.6f);

From b5d3355dfb513bbfab362734a672f8c51fc10402 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 15 Oct 2013 09:22:47 +0200
Subject: [PATCH 04/47] Fix accidentally comitted hardcoded autostart

---
 ROMFS/px4fmu_common/init.d/rcS | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index e0813b0319..93e76184d4 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -166,11 +166,11 @@ then
 		set MODE custom
 	fi
 	
-	#if param compare SYS_AUTOSTART 10
-	#then
+	if param compare SYS_AUTOSTART 10
+	then
 		sh /etc/init.d/10_dji_f330
 		set MODE custom
-	#fi
+	fi
 
 	if param compare SYS_AUTOSTART 11
 	then

From 71ac33596836519a341001bb48a8835b8af75cd3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Wed, 16 Oct 2013 21:43:11 +0200
Subject: [PATCH 05/47] Small improvements to autoland, ensure that throttle
 can be shut down close to touch down. Depends on accurate land WP altitude

---
 src/lib/external_lgpl/tecs/tecs.h             |  5 +++
 .../fw_pos_control_l1_main.cpp                | 32 +++++++++++++++----
 .../fw_pos_control_l1_params.c                |  1 +
 3 files changed, 31 insertions(+), 7 deletions(-)

diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 2ae6b28bb4..4a98c8e974 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -94,6 +94,11 @@ public:
 	// Rate of change of velocity along X body axis in m/s^2
 	float get_VXdot(void) { return _vel_dot; }
 
+
+	float get_speed_weight() {
+		return _spdWeight;
+	}
+
 	// log data on internal state of the controller. Called at 10Hz
 	// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
 
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index cd4a0d58ef..3697af225e 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -196,6 +196,8 @@ private:
 		float throttle_max;
 		float throttle_cruise;
 
+		float throttle_land_max;
+
 		float loiter_hold_radius;
 	}		_parameters;			/**< local copies of interesting parameters */
 
@@ -227,6 +229,8 @@ private:
 		param_t throttle_max;
 		param_t throttle_cruise;
 
+		param_t throttle_land_max;
+
 		param_t loiter_hold_radius;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
@@ -342,6 +346,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_min = param_find("FW_THR_MIN");
 	_parameter_handles.throttle_max = param_find("FW_THR_MAX");
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
+	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
@@ -404,6 +409,8 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
 	param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
 
+	param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
+
 	param_get(_parameter_handles.time_const, &(_parameters.time_const));
 	param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
 	param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
@@ -625,6 +632,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 	_tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth);
 	float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
 
+	/* no throttle limit as default */
+	float throttle_max = 1.0f;
+
 	/* AUTONOMOUS FLIGHT */
 
 	// XXX this should only execute if auto AND safety off (actuators active),
@@ -634,11 +644,12 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
 
+		/* restore speed weight, in case changed intermittently (e.g. in landing handling) */
+		_tecs.set_speed_weight(_parameters.speed_weight);
+
 		/* execute navigation once we have a setpoint */
 		if (_setpoint_valid) {
 
-			float altitude_error = _global_triplet.current.altitude - _global_pos.alt;
-
 			/* current waypoint (the one currently heading for) */
 			math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f);
 
@@ -712,16 +723,23 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 				/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
 				// XXX this could make a great param
-				if (altitude_error > -20.0f) {
 
-					float flare_angle_rad = math::radians(15.0f);//math::radians(global_triplet.current.param1)
+				if (altitude_error > -10.0f) {
+
+					float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
+
+					/* set the speed weight to 0.0 to push the system to control altitude with pitch */
+					_tecs.set_speed_weight(0.0f);
 
 					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
 								    _airspeed.indicated_airspeed_m_s, eas2tas,
-								    true, flare_angle_rad,
+								    false, flare_angle_rad,
 								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
+					/* kill the throttle if param requests it */
+					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
+
 					/* limit roll motion to prevent wings from touching the ground first */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f));
 
@@ -785,7 +803,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				_loiter_hold = true;
 			}
 
-			float altitude_error = _loiter_hold_alt - _global_pos.alt;
+			altitude_error = _loiter_hold_alt - _global_pos.alt;
 
 			math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon);
 
@@ -862,7 +880,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 	}
 
 	_att_sp.pitch_body = _tecs.get_pitch_demand();
-	_att_sp.thrust = _tecs.get_throttle_demand();
+	_att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max);
 
 	return setpoint;
 }
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index d210ec7126..9b64cb047a 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -72,6 +72,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 
 PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
 
+PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 0.0f);
 
 PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
 

From 013579cffd70f46788a356043563340731beabae Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 17 Oct 2013 07:54:04 +0200
Subject: [PATCH 06/47] More improvements on landing

---
 .../fw_pos_control_l1_main.cpp                | 72 ++++++++++++++++---
 .../fw_pos_control_l1_params.c                |  2 +-
 2 files changed, 64 insertions(+), 10 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 3697af225e..348a17ba63 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -727,15 +727,17 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				if (altitude_error > -10.0f) {
 
 					float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
+					float land_pitch_min = math::radians(5.0f);
+					float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
 
-					/* set the speed weight to 0.0 to push the system to control altitude with pitch */
-					_tecs.set_speed_weight(0.0f);
+					// /* set the speed weight to 0.0 to push the system to control altitude with pitch */
+					// _tecs.set_speed_weight(0.0f);
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, _parameters.airspeed_min,
 								    _airspeed.indicated_airspeed_m_s, eas2tas,
 								    false, flare_angle_rad,
-								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
+								    0.0f, _parameters.throttle_max, throttle_land,
+								    land_pitch_min, math::radians(_parameters.pitch_limit_max));
 
 					/* kill the throttle if param requests it */
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
@@ -814,7 +816,7 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 			_att_sp.yaw_body = _l1_control.nav_bearing();
 
 			/* climb with full throttle if the altitude error is bigger than 5 meters */
-			bool climb_out = (altitude_error > 5);
+			bool climb_out = (altitude_error > 3);
 
 			float min_pitch;
 
@@ -842,6 +844,43 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 			_att_sp.roll_reset_integral = true;
 		}
 
+	} else if (0/* easy mode enabled */) {
+
+		/** EASY FLIGHT **/
+
+		if (0/* switched from another mode to easy */) {
+			_seatbelt_hold_heading = _att.yaw;
+		}
+
+		if (0/* easy on and manual control yaw non-zero */) {
+			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
+		}
+
+		/* climb out control */
+		bool climb_out = false;
+
+		/* user wants to climb out */
+		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+			climb_out = true;
+		}
+
+		/* if in seatbelt mode, set airspeed based on manual control */
+
+		// XXX check if ground speed undershoot should be applied here
+		float seatbelt_airspeed = _parameters.airspeed_min +
+					  (_parameters.airspeed_max - _parameters.airspeed_min) *
+					  _manual.throttle;
+
+		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
+		_att_sp.roll_body = _l1_control.nav_roll();
+		_att_sp.yaw_body = _l1_control.nav_bearing();
+		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
+					    seatbelt_airspeed,
+					    _airspeed.indicated_airspeed_m_s, eas2tas,
+					    false, _parameters.pitch_limit_min,
+					    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+					    _parameters.pitch_limit_min, _parameters.pitch_limit_max);
+
 	} else if (0/* seatbelt mode enabled */) {
 
 		/** SEATBELT FLIGHT **/
@@ -861,13 +900,28 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
 					  _manual.throttle;
 
+		/* user switched off throttle */
+		if (_manual.throttle < 0.1f) {
+			throttle_max = 0.0f;
+			/* switch to pure pitch based altitude control, give up speed */
+			_tecs.set_speed_weight(0.0f);
+		}
+
+		/* climb out control */
+		bool climb_out = false;
+
+		/* user wants to climb out */
+		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+			climb_out = true;
+		}
+
 		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
-		_att_sp.roll_body = _l1_control.nav_roll();
-		_att_sp.yaw_body = _l1_control.nav_bearing();
+		_att_sp.roll_body =	_manual.roll;
+		_att_sp.yaw_body =	_manual.yaw;
 		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
 					    seatbelt_airspeed,
 					    _airspeed.indicated_airspeed_m_s, eas2tas,
-					    false, _parameters.pitch_limit_min,
+					    climb_out, _parameters.pitch_limit_min,
 					    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 					    _parameters.pitch_limit_min, _parameters.pitch_limit_max);
 
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 9b64cb047a..c39df9ae31 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 
 PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f);
 
-PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 0.0f);
+PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
 
 PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
 

From 95aba0d70eae6a9aba55d31f223b852df1f82dcb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 17 Oct 2013 09:36:20 +0200
Subject: [PATCH 07/47] Almost perfect landing approach, needs touch-down fine
 tuning

---
 .../fw_pos_control_l1_main.cpp                | 47 +++++++++++++++----
 src/modules/mavlink/waypoints.c               | 37 ++++++++++-----
 2 files changed, 62 insertions(+), 22 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 348a17ba63..219c6ffa6b 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -635,6 +635,9 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
 
+	/* not in non-abort mode for landing yet */
+	bool land_noreturn = false;
+
 	/* AUTONOMOUS FLIGHT */
 
 	// XXX this should only execute if auto AND safety off (actuators active),
@@ -717,23 +720,39 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 			} else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) {
 
-				_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				/* switch to heading hold for the last meters, continue heading hold after */
+
+				float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), next_wp.getX(), next_wp.getY());
+
+				if (wp_distance < 5.0f || land_noreturn) {
+
+					/* heading hold, along the line connecting this and the last waypoint */
+					float target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
+					_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
+
+					if (altitude_error > -20.0f)
+						land_noreturn = true;
+
+				} else {
+
+					/* normal navigation */
+					_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
+				}
+
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
 
 				/* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */
 				// XXX this could make a great param
 
-				if (altitude_error > -10.0f) {
+				float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
+				float land_pitch_min = math::radians(5.0f);
+				float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
+				float airspeed_land = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
 
-					float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
-					float land_pitch_min = math::radians(5.0f);
-					float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
+				if (altitude_error > -5.0f) {
 
-					// /* set the speed weight to 0.0 to push the system to control altitude with pitch */
-					// _tecs.set_speed_weight(0.0f);
-
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, _parameters.airspeed_min,
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
 								    _airspeed.indicated_airspeed_m_s, eas2tas,
 								    false, flare_angle_rad,
 								    0.0f, _parameters.throttle_max, throttle_land,
@@ -743,7 +762,15 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 
 					/* limit roll motion to prevent wings from touching the ground first */
-					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f));
+					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
+
+				} else if (altitude_error > -20.0f) {
+
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
+								    _airspeed.indicated_airspeed_m_s, eas2tas,
+								    false, flare_angle_rad,
+								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
+								    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
 
 				} else {
 
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index ddad5f0df6..adaf814047 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -399,6 +399,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 			if (time_elapsed) {
 
 				if (cur_wp->autocontinue) {
+
+					/* safeguard against invalid missions with last wp autocontinue on */
+					if (wpm->current_active_wp_id == wpm->size - 1) {
+						/* stop handling missions here */
+						cur_wp->autocontinue = false;
+					}
+
 					cur_wp->current = 0;
 
 					float navigation_lat = -1.0f;
@@ -419,13 +426,16 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 						navigation_frame = MAV_FRAME_LOCAL_NED;
 					}
 
+					/* guard against missions without final land waypoint */
 					/* only accept supported navigation waypoints, skip unknown ones */
 					do {
+
 						/* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */
 						if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
 				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
-				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM ||
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) {
 
 							/* this is a navigation waypoint */
 							navigation_frame = cur_wp->frame;
@@ -434,17 +444,20 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 							navigation_alt = cur_wp->z;
 						}
 
-						if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
-							/* the last waypoint was reached, if auto continue is
-							 * activated keep the system loitering there.
-							 */
-							cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
-							cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
-							cur_wp->frame = navigation_frame;
-							cur_wp->x = navigation_lat;
-							cur_wp->y = navigation_lon;
-							cur_wp->z = navigation_alt;
-							cur_wp->autocontinue = false;
+						if (wpm->current_active_wp_id == wpm->size - 1) {
+
+							/* if we're not landing at the last nav waypoint, we're falling back to loiter */
+							if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) {
+								/* the last waypoint was reached, if auto continue is
+								 * activated AND it is NOT a land waypoint, keep the system loitering there.
+								 */
+								cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM;
+								cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius
+								cur_wp->frame = navigation_frame;
+								cur_wp->x = navigation_lat;
+								cur_wp->y = navigation_lon;
+								cur_wp->z = navigation_alt;
+							}
 
 							/* we risk an endless loop for missions without navigation waypoints, abort. */
 							break;

From 5fafe45745238ae377ed748c6f561a6cbf02221a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 18 Oct 2013 10:14:42 +0200
Subject: [PATCH 08/47] Updated docs

---
 Documentation/fixed_wing_control.odg | Bin 0 -> 12258 bytes
 Documentation/l1_control.odg         | Bin 11753 -> 0 bytes
 2 files changed, 0 insertions(+), 0 deletions(-)
 create mode 100644 Documentation/fixed_wing_control.odg
 delete mode 100644 Documentation/l1_control.odg

diff --git a/Documentation/fixed_wing_control.odg b/Documentation/fixed_wing_control.odg
new file mode 100644
index 0000000000000000000000000000000000000000..0918edcac86ac046cda417e534551bb91480c219
GIT binary patch
literal 12258
zcmd6NcT`l%((jOi<RDRUk~G92gCdfH<O~kO2*@x4Ll((Mjw)Glk_?hFk`c)QB9c*Z
zPLkikbG&-)_1t&9KfbqKt+m(MGhOwo>gwLT_piGisGy;f0RY$lfJmw4y)-o54lV!y
zaD8C|00281JE)VpJrrzj53_-Qooo<rpbOlR7Xfy(apXnVL*bSPh_f9O?!;?_1lwEN
zKpY?Z1@Oh5g1Ip|0C0VM2ZTC-dEM+_k}_^~Fa{SW($NM1mtx}Q<zr%i!XXGt8@QDe
zldg_Dk0_IjG_EA#(IXoORQ!h*o@;vsH#->IQT&f4DJEwmT>N`H;&8AX)KT0CBK~V&
z;=gK(e|Pl9KW;D^xUCeEwUd**I1uRS>dNaX$csQ)0r|zm#DL#hnf_F@g#1g--Wdt|
zu4M@ULSfMBt~&zxdHI1qbgv`)r$Jro{u+`!66y$rJAKddKgIaf=2t8P0`ae)uk-Oo
zy1s`lz{e*9{NwkJs2%NWVE@$PwJr|?VP_9^vax{uC8uDxjom+mxz_#O(_f>uLRwnF
z{uTExgCfAkCkV6xT7sRxJT5j+SLPp)|Iv^C-dnMM>#Zdc?D}6V`1yd>I!p|Ipo{-9
zQ3aT!f6U73;ggp9K8O$`^m|`;EWu7tX#qZdK^}fS9)1BGegSbl0dXNQN#JkvBrPH0
ze>D)%5fBs?77-T`kp%u|{KEs((#FXKZpCBijQs9N`hkvssG6{d_MgcA(f(I6h&u!Z
zb(9wTH|;<F{zKOa3Wp-WP6(v5s*ME_s`35V0}AqjSk%A}4Q&H+H&GD7FHPq5NW^0(
z#0dx$5CaSF3-Jh8SU@dB1bFyB0{oUx5D!>PL|8x+!Xx^K4+OIO)Adg;e|Y`Zv+a02
z@g1FPAPnDAg|N8B=7Aub;Z9OaAR#88wB(=T_-E#S{Q4Kd_W}810m8<{{=T9Cei1wX
z0N@C9x=y3x^&&KFH0!tkBJ$cckJ&xQ@MJhw<gMfLrID3wr&Y3Mm@u0SX!4;pT#s2I
zx;%Rkt|V3#q9LE6z8_6rec)L*_V{@8d7nlpqr?*w#@l;w`Z6W%3ay@a9BEaJc@Cv}
z!)G5ZS4r};*X{}4ks<Ic3`Ng5Aac*nbDT73g1eW`sj)nN$Hvasle?EpiDIr{3y*}e
zSr)RFhVL!MiRg1a)+-F_TyS$hhFj<ro12*Eyr@~vhymnh1S{k5%GZiny%<-J&Z)@c
zC>&1SC*JM176ez7b?nF&yBL{XG|}<gdl!mKEGMULSDcar&z^+ZQ{Q~X4{_CNdh@tt
zlft+f%a8kL;Pphoqj1-k@GW^o;l?9zuW|5bvuaA+sb&VQEUC7_Dx@MeE)OReNOlu3
z{1w`N<~2aU8PP6|+C#~VxX|KZzIcxOQZ+cR9zSN9K<`J_ssjb<Q?o~mj;weO-RZBG
zqhVjRvdWo9_zrwc{5qk*tF95cpf5|J%Rx9t?q|nqV7aX;pb(U2aI>89=EcK4)%*Yg
z^isRFHYANVsnAN_2YTZjQaQ{3kIma)l6P~*=zDhb^r%fhg;#PAA#V2_<qp>2AWKYF
zx%V9LTZ-?GQNBd0H~>VXR4*~j4+FebUO|ITiRujBM|bYU?fCd^lwlZ(<C&AzpT^tX
zeZ=hfA>qYl9at#)6$d4gF`F073RVH+n*ix(kWs&jSc3_#-b;9o54ZFDR2MtRfO=^q
zvBhdt;j<R$>J1px8f|`$cV`mH=^#-fLiy11wZ{gBDS(X^v_sd>V?WWcce2I*h+(E!
z^ynL#q4)SRw+5X7&6O;OJ6yiiN!h>E#|q6jZnjxfcAfwYt7N6mHoWRZL-^?+#7$QO
zrN6ihYE9y|^=gMoa$(J4o`Fpv9Frl4fzBbO2`A2Cw6=4jRdF>_w!>wGMQlRAethWo
zwTp5$qrDnchp2ILTwK@HAXj_cBA0*8iF}(&3^q>P_=Ih>^A-J$RCT=Uw#{n&;106K
zqqsT<^hB8dbnr|R{Vo}95bZ=O|FD@DH`d&@Q()0zVY2f9G}9HoA72E?b1bW$8({5(
z_2v}VvEyqM?9T+UlKc$ATWV7Gz@J28dA*>PGv;r=;(Vr1uad%H(Z`Onki>y>Gj+Hh
zYqs*SqiuANA@6W^A6O2Wn|PVdIZEQi?O@Se01am_S`Xn3%+-E;>H<-7e;}0@Y`Y$N
z;*vcfH&85wrlOwkJmI#Qgx2Ka%$aWwK8BpARnvvy;G@nVru3V?fQFuc28hn{OHupE
zwk%-Z93M2)#O(!OE4w^_-G1!bw?zJ)L*#<pHh_k1BgGsv*kqoqct<$FIe2hG(p%*v
z!n@>a_1K;u_Dsx^!65!)-Ho-8v3RAcCEVRa-{hNHY;VbT(@Z}<5D_~9WvRSaF<Qwb
zPE0?3{pKYV1`hlM_hZpl2lb=`{$AS!SSftrx6x|6sI}RTlJN>i^PR=ZbMBK!qT)nX
zY_>TC>rSJ@l>4wZk9sLXb>^ccT|7Kby97}(*1Z)4fWdJSyBTrBf_F1dI0)|P_X>w(
z&qCT>e{%6i{Q3a(MrJ8j-U*S(L<frDmP^R^aEHC-uq=fkEY2`gU{QyHg6MV^woYR)
znRlhIG!t+KdYE{Iwk`0IcVMlD9q+SAAnD45tGmQp9>>t3l76c_9=PM^L%}(Bn#X+6
z*17pT;cXe{r&+)Q6)fy7)rZT37yy7RGXU`S3+W92002S2ovy$8-0WbRrjwD2wd9_I
z`b4R!e94wEWUS=^x)D@(nC`C$s8{J2dQ|Q~`>G#V_Qot3eWloo4w75~?qQuIGMq%2
zXocPGef}jU&Ly7)8eeCY>fz1{RH|$CbZo&JDyCrfA~TCDgNH{`sAuNW4*(TpJilo=
z$|Fj=_U(j{Pe`{dsMW?*fT~qWWVt($=`IN-mUZ!2&a`H7@C9~?jD~~fVNOWm;>NPq
z&KYsd(Vo!TN~c5zCA>EZtF*VJH;oNn7JqO?oE~%ED6GmW_n4@8!*L_MV9ZK$_T8~k
zdOy=0&pM=b3tOk>Tu_Ti<V~CUddjHnt*rxz0)^eD;bO~>L}OgZ+kM634h$-4>=59N
zoK-L7x|_*M(<k{;`qG+u<hM<Lh{jqGA@~}ovOcs4J+#&S8TiOnu~@sjbjlsEUY}Ca
zDrs2jfRK3ctT0zL??UaF`dGb+h-u}@1Xas3)swxWClLMJHwg1g7)F88M(GnmL9XKQ
zsLa=7x;i~<4P^0@fno#FTshm~VY8iHYn{rwN>;leG^)xSSzkay>9lnBaQYj_+050M
zP~=9me8Vd?-q;J+*_{p|N@q20&Wucx9l27BpYthA+xx4&Iq;t`@_EA}zaHp?&?bV?
zXtUq!8VVJq>7L;X9o4#9aj6b?!pJHg=0sJgozTM*I<UCvWUEOO6zMzQqgrV^+i(Wc
z*;Sto?6nQNo$sYjZm4*OkwRX^e-o-Kj-ov+TH<DQI&N4=ke5g&0BAakr8r~i!?&yo
zQ@%kSwcv60t|0VkDhWlY3W}<SRl#Ls%28EFz-cyfY^(nMwY7L*jR7QH>G_aT@}Bg$
z4fmeVeA-*IFH*PHNd)RSZ!%J?vMdU`HO-4D`y9g7q=-DU`>M9Y_0Eez);uzoOY$A*
zBi{E(Sc>n?-&7-)XRJS)In#+03_x6~cG&Oly^H7^QS=&mqimtBG3_SK`KiCqnya-Z
zOnX*^$c=V|H_~hOApsV#TmK;52_hf>mL3b(kP1QdF35%fx#rMro7fR|UY5oTMSR7)
zq-8jFPszUA$S~W{Q{<l{Gd|^OYHWN7&`a(fPO08GKS31|arBpK6VkI^LyeFmu-0nC
ztLoFFd}bz7_CDm>mBri4C^*>lQm2R&Z~EbjFV=aRJk#GOfPn;H;fj0}n4uvL%6YSt
zjCOEl2D|L7ekLDV&IL-I2t8SD?i{TVM5YjVSOAZquZT))28Wo#xJpb_kV`*%GVTRO
z4#@6Zrr-&prXF3s*hq2FS^Iw7GfQ`$)e*uzT=~cfMJ{d|nWvMN-{eJPJYF0OXiyA`
zlQYnQB92nI;<L6b0ND_NVKc|qc8?n+7i;JY#bOQloL_V*s*T9K`?8`Sb0AH!xJbCu
zn;beTO|raLOykXRzG(emhab1v+|SP&cjRM=Zsk+0s;%`!>05_l8che3;}m&%j8{(Q
zvnK=0u%}v*!LF~PTx_FK4czhXb9}O0B45k0dza-=u;l~rUajjDKXT!OQBa-*pO7&+
zCxcCwL5F6QOkVSYYv$&+x)&8bF<DP(Oe&m|a(w8(>e$P(I<Y$y4G%k}Iz<5=*;cpW
z$8PZ5EqLJ>zGK>MjIvXLUL8=Us_7eO)+rk1t0q|vX;EmwKOWuCuQuOAVp>1GA0gxR
zwwQgR4EFd%VSm$5*aeP3e@YrlED4uP3P6jf>}+o(r)3EL(^Kh;zCh9|eIoo9x<<Ew
z&xJ*Hr^|S>X%8`GJX5FX12An4Ybfo99r?yS=a6?Lvvvu;7|;?7GF0PAfu_>7I`QVA
zgTe<Dc*YIc&gRLql$&Qt>bRX#f)5o%pXGC8^jSRx>Bok)cF9H}b2$yQnw^Lh)=!w2
zEBzGLx^{1Ih^z>OiyGC>Uj}OCk~ej$3~Tx-ys^()z?Palo(^utOt-jwrB}C&911&n
zljIwZ`iYvmOV<5Lp;P&m4m{=RY-0BEY}?x@yq+ULk|n<eM$p_dduIql`07+LdWLnd
zm_U}wT*2352+4UdV0yoIyL5uodoA-Ao?wY%>d&=aO;E}`EZy2~G9%p~(Bc`*b3dEP
zt#7QYe?v)_*ih;SbvD_qJY~ftc@@SLJ(%Br{>eXrdOcWQv6r4&-30IC%jqy7{_^bY
z(j;nDFPB&@!+Ri$fn~Lup4^_GTs`(Gp**PfQ|k3_tKsGSNk>SCL+WDugd&<uuVKC4
zTVc9<4AkxM8%Ti`L##Oq&A8hm85f#I{Cy)GO>g^#y)RFWNd$9<F~kfN&<TA#(8Rt}
zhm2M67PMc``K25a_9eeQph~gL<$TR{X0k9MJR)V!pfDF*^chEJtv~Z4*Bw!rfR$dN
zHh~dInSl4COiQm=Wj8FbiKiu10*YQR$_<jxpE-Sdbz4J%{a%lxcIsDlxfMxwqgPC6
z*i1(@#8uBzN~^kyYZ~{%ldVr5c=ePzntO7&E_UW@!OW$NlfwpUB@Ns-2sxITcFws4
zh>ag+K1@CK7jc~M#kR2Y&0OsjM{)1K{QS-q#DNHlx5ABWk9NJUJ|`V)l|9E}ls73R
zpMP`ICG@QB%cZqDT~Zf3!IJQ#Lvt;c?o^gp9D4I>ZZH0O9H=>yA!l@5HC%j%ZlNBE
zlJnFMoukk}ni6Od9Hfy;WO-i&7(s72xNtjkn;F{=&yvB9huNDaeQ!}TG`8aTySs|1
z#<ZfbQ-xH)Z$FLd#n9)5@VRsoN%4`+gc2IMPO|5$GfgwyFTt%4Z6L-+#wcy{bJd7o
z(Cek6*I5<k5CH@C_h@V7z^wGOPr?EiCSoH+wlPm{oMs8F1d2(9JTnO2fHDfB@pEBX
z_<|qWFhts=y}!e0<Rw_~AY({G5X*~?S7tA?nNg*&Ob??aQS;?R!a`oAVR;y~4vF0U
zM;;)hoVNE@k8&YUd#<>H>S#6od+X%2+4nkMR1zPvF?=vj-=C;@`cNS!$<X~0`>_Gm
z)|ZtUOzMb+$8%5cCu6EB=q5)-^*>UQRy5ERPk0D2uh_x_vl>$COw}`DwnB?0Gkm+U
z8=mWyvJ@}ag6XHpH|?5i64jjboYu%MCMgh)K&P1DKADJcU_;@4F{*6?Mi>4IO>wpr
z8l3&e0f#W+!<8nm{F|@(TBqf$aD|INF>)#Y4XAFyqfv&u_6=94E#(F%LN801H=3+=
z!2^Gs_Y%oNKxn)-ea0;VNq?R|^WGU9@*;~enfg&mxo=mUGrDc^!bpt$I$4^Ct)ObH
zy6U@pH4ZSEEn$&V*-`5!jk19m5t4Pw4K)V&Z;qW{<r$(tDR)Xe!ViT<^DBY1#No8l
zAayuHgW6n<+Zm%gP2<JfCKlq#ro+17$n4hqy6=dZS3Se#i>w1i9~utN^aI}!+n1?1
z<W`S82gg?&XlE_I3E*JIBc!E#h{5h&Pe?`vAasc5M+C0n{_Uw$J3D$i@AY;R33o)n
zYTszPwNQ%MmKDE&OlRbdF_KO^-XPo0SB!|4(dbM)<I}LhNIEBX3Mdb9t9BoC07tU<
zP$s&`3^?Y3u_`|G6}7%}!;sU?6AKQanmAqto5ZP;J%FXm`<5vml))1sE*k2g{3-fG
zOJaUlNxDZ%L&IIHbY+v6SP~rxTjlmJ7co`tWm4X|49~OQ4H9P88s~f*t|eP9XQ?b8
z5if9S<TT~=jCTX>=c+GX(m#5bWZ_Rqi!K<HEZ%<8VN{NrPmE2iBR6k~*()X)vt4?p
z%!Us}jhBqMTt<l3sJ0}}SL+g#B7z`HZbCy4HcIUr^|cgN_*aD4hp&7t3%nsw%1=E-
zUP`m~7N;QjRrAc~G~)vr+RM2ete`6!^vBaL;cEl~M-xjm70r6e^cJ<gBUsAx98LRe
z>QgxB&e(Zjq<3C<m#R9TTaOQLB*OFad?sDLy4*s=P5s1{;TbT&@BC<2o+yKcV0}+K
zFj7pILzv%xE_AYT!KIa5^O3+~(a-4y9UMKwJYiFF;T&vtx&$s^_0%sWN9YX<XAY2+
zLPR;^0T$YVjN+B~fr-y*LxiodG<T>1n3MVXnXn<ScAXkx=mR`K%diD90lY3@n)3(<
zvplgsP6U29tWP!orL$e)5dkU#kuc?(1YZ~o>w`Q2f>X|Vo^?90wR-F0SOd$Ey^V#V
z4Qg^NPCD|8%|BX~!Sa+p-l0IGsUG*m@Yn=!8;L;_KYib%`uX%avJ*l?VIXD85w}X_
zN<x8op9WOJQ4WLe;o3Q*XpiRl$#YNLH0MQqnMwL4r4SZG{E*aSo>P^LthNiKV1X?<
zETr}>k+7N^4-F37BAWfEmUc?^X52;~_Lm)rN=!?1ZNsm`VN66?59pS}dKl3iy3&i%
zr9xHN3lV5GA>wjh9T@nn>5Y5%>)x}gZoEXPOC%%q?{mSXkQs{d=f1a-%i4)<5ok#?
z8K8Fu74KZ(4lWHA(~Szaz~}pZ1N>$MCDz(0O@Rwmoi%kS8=!@89_0sO{br#b?|ZdI
zDs8@d1Wjc!AA7*ALL6HiAeKBHyG9N=@@q9`<uN(e9#&W+@2E_T@Xwz#fC#}qtwm<a
z8Q0hP<9v%=j~<ltQgw#hvmS{&;TJickzz!5Vn<&fNEi!feIvn!bTGaE09IDksI!*T
zpT_ehold4P%X}OT1rBja8Sbmp;-ZP}sMLP?EF$yKb6xdtX5g#*>w_5+lti-0Ad`rW
zuD2vwtOqen;#C-d+T;!ssEbhUjxitjjj~dAs+ny)$wJAJwx~T6D<tzw5r!+wEx<~n
zP=Ka<dWTHo3ue869X{a>$44>PjVHQU$j61cE&)oWEv>hF)&_DGK5ju<%_nQ$BMA-f
z(l&Nz-g<W4qjLg-qOizpeKL@$dukFpFeCZlKp?x2TW4vnPY+}13^(CweyG^CV}`<H
zPhPH~S6-)q(+2HbD5IXfNq_Ir0Ig{q*Ak|UXWW+*nM=3e0SUIaq$x*9*Fg!o63Mcl
z8w#0p4j*;3+a$1FqQ~q6jU^OAA4(wfZ#mK-){O`5mo1=i5+?TI_wFMzYdtaATr#mf
z?(nEo7_}lMK69{*XGD^zvWGv|$XOSabXw069<ejm*g)#sig9}e?Sza4b<Ja`bQi)a
z+0v5EOi1*;evIo6|KylD$ecBa17U4Q3y%_JrJ8+HR;;F!Li4yXV0YfuSd6tZZTDV=
zhsWNg;^*f2=?IKzQMNBb$y2x_MVHf5-E(CJ_vZ6rC<ZbqMb8hcX|1SUZn4;!O~1mB
zjAUAOpVN?HF_~0p&x~4Rst1qd^;VqlI>$dTxtbzgd}kiVQIXaAWVKfYNu0amb?AM~
z=pk>d?Gp(B0I9?ujQ+=AkLP30F;)x!;QFEj0027H&UO}Xuno)+==9^3*B)*a@<3ey
zAD0>z007`CDavU90Dvk000n@J3IG6}+)WP$0MIv-<nHRYz1p0fhi4i9e1_HEY)hrE
zr`g37@rS1A+yx%IqNnN8sueH?S{t7Z&bD~V%msSSNC5(K2}7khPSaQXGyfNF-mRa0
zJ6xYQ2^yUxPc-cjy?N-cTUuM9&0$WMdj+onHrr~_6J8~&flXD(DU2ne5Xwu5Qt<3{
zDf`c*5-soP8rGjJkM?o}IuG8Sm6lN_gsWBhOU7ftrn+sU-8M*%Jq+l*7P6?05BnHG
zUJ=eck>qcziF!)Dc(hyo_DfH(ZTgo*Ln!5ZFL4QcMOqrvKr7a@bcLFA%x}1BBG@wl
zrD31-JlN&mQJIBIsnOFfHXU5x9DV4voUfCYFEnV0qA>9#+#5M&H)x5PXcS&TE(ssG
zdWoHEed#$dp7Yli9@qh`F2-sAa|_qM<Yl{P_SVx$Fe65~!=f%XB1;InJ4HgR;%Qqq
zj@vb^)}Qu1&ug4Nfwrpjd~?WVu6kpsZu*V3W)1T3-c#F$K{in89h;9Dk=}*UGXm}h
z6y1Bfw7Id%o0q2t7A=J>mCa-CmY0`L9=CX3UG6Ur9xfUZmOh<mZ@7FS*>D+YKHF?I
z$h92wZG1k}bl#Y2wrZ;+YP`EIx1{uB(aHYUi{LcrzSq0+-8c~t!P6#Wr8FDj{3OJ`
z3i)0d^JI4pt@8S&a#XK+bMer;!=HYuH`0jTyKxf6wHS~YWoO!$YILcxR@1C2Bryx4
z7$1>N71M59*K%0D@Vd0>#m%d^EpsK}Daw;3-Dq&~w9h<hu*TlVt7*r!cUm*0;mm7&
zM7lb87j&xLU)*0()xbKH=sCK29#J(JtD!w$L$TiMeOi2&VvZ@?w!Tm3iDw(FqT`8Y
z6)nIPv<Z2g)o32H>96}wCTqCIRr(~&dsD=I@2$=}8AmQ3l<lUYgzhtgJ+$$fExx)P
zZFPkc3yF*QwrSI=`LfB`wdS+<<$>eFrG8YbgAeBpo_8R<H3UO#64otq#|LRt?K6wt
z+;XGB&X!EMx1G1(E-n(~VL^xAD7;RML~hUy4K_)8?$HIwbex^JEjFdkyh-t7{dOr$
znfSl|eEmOog#Z6>afO;NV7?VawwZK&w4`)jU9Lp-q2FJRy?;4$0?-2hfO`n|BO9wf
zPwpHAfHrnuE2tw-*2c*WZ0`vCdt>U~7~3Q4o$c+QaOeM_of859vj@YWu>Yam^|_q0
zqXiiGU+idq7bg;7g@ii(Uo5ZRJpZ1a>q!5*zu#G1r{t&A?+yUjf8z@bfk0t@9D4#G
z&Pe2+d)jE<x5KZ;@%k-yEq^~qzYFFO3;}k6{z%#Hg1LU5Iy$+-pnsf28c#<~fo?X>
z<`L;Qm3!P>?@qPc=+bsFCNpKg31w@d8zO<0XUO?xb}crxnH^+OoZURbJQE1p^f-uq
zY56V==DwKNtB#5e8dTVQ6&19+L(5g#+&q}zs3520-t6QbvMtUM<)*rSIC61valG>B
z;`FOVsoV3RU~UUKU8-JI&M$E>Bfh=vb$o3(#vxDNIw<wsj}+A6wXB@O=uK9-LDMv(
zl1O|}Rz52HF_ovciB`f8EaABO`1~C;Y%mgICOoC6tTkUsY97Kq8;{3IdtZ{PEZf8A
zvtW|9L4E$q&bEC}a=)cuihcQ!-^tT08=FG=4-28nd=a6{QGA&<xtbo5xZ#7CNutIy
zz;>~FoW{ibNz~lCZ%c0ay#$~*#+pZZL`&r@n!BzK&p6l6rHd(-Q*K&Oe|C<wk(|-Z
zEte8$7r6Ct)^-~o`%ng9wtcQ_37OFPNT3^&-f1m}|0#7)456z_`W`&77T7Ga|K9aR
zlNjzSHfKoL+_0uwfZ+#$fePsn0EL^+oh9|Ur?#&0lh%bO5_2q~d0F}TAJO#5L(lKt
zD3!d6%WJ8kC?LJk#tEho&`K<ykIu-(cj8&7rNCWNmBOaCcN@ONnhgA`4ieqM-j`As
z$~gQ$hW`e#J&fbKeOuMz>04o!5ZR(Bowyy&sD5cm)LzfK=(<er4=V33<D?`!h|``_
z7&uFlWWC2Zdq=KVJ2m#vz2n^8e74w`My*e6h7|O{_6Jru10xsm*aUia9vX2ZU3G28
zmu!DiUy;zB4ZmmEi%NOVms;iCRo430AXm;DJ$9%=;wQq3)UQjR14N6z14*Gq&HUl!
zt(!IT=i|aX32_+h#+AwQl(qX5fXgcRu{z^uMbLgNH(J^7WhiCu1ZOD5e!TfsD$t>1
z3;)SZKv$`4#BBcIhamUTB;2po;esvMff?Rf?P-Z*qN_wxg%)9s;^~582ke)+M9?_y
z4cT@^Yg-*b0++-CniuH$Ma`eZ9&x`V+N@S_<*|<$>9H$2bk)HYb(htu#pBd3stXZa
zjJ|5AJsO*R@+ClETsbqKQMJT`eLf6d5_h-F^7Iw+p=X|sFUEM7bKD4%b3IWB3-Cq}
zUMBxMW~gC|MNJ^NE?uuOy~D*uj`2v*gN%-z`9QfLWg@&9j-j*yd_Iy`uep&x0lN0Z
z#5>5bU=o6whe_RXokq^g;~JWwE6I+2hDt_n7?3FWjT~<|5g&_gmTZWZRdeF_aaq0>
zQ3RR^-oAfdDB?-kWQok8U%Yu!5TBknqPOcL#7b1e|JDsWx?&{)e!}O44{8ZA(*;o3
z{g^e>&h6qjR}~moUy*A?@xLnNY7ETy=);6vq~*gqdpT6YlFf~Gc0rd{wnUsl{jiRQ
zwU<EGc1Z$*Ct#!;g;H&ur!>~il73WP%XS*O*>fr{N%=DTbJTG2GlI2Qra&HR#wJkZ
z$jdF`;n2&=@#zz@<yRu=R`gFuU+cUIE)SmGzvoNNJ}>ra_+wRjdtjG1Uw*n1=i`KM
z?E1Ed`V>)|DYx+$1$z?Rd2pmT7NGl}$BA__xyVA#_N_hHD2X$SKY;ip@~#Zh+1uxi
z+{B!Gi%w2<uU=XhYIi}sJQ3kb&bw|oFU!%ncysC-k9sMUvOy2c8CJxrSRSEi(XQVB
zMKH=aV-lN4D>Dsy#z&OcPjP*o%1PF*fBh(0Z-|)w4H$20=Dp8wfkn7tH}+WVfW>Jc
zF{7>u&-B&#{>1ovQILUj-m;~T0$%Bzk*&iU5}lht;}^9j1TH(%AY+HGsiZDy+^<Ym
zRs3Avj|jS9%lKlY5D$mm@l>EDNu|Fx8pcmXDdX*e*U#HCbXZ}zvSxwz?8Rh-<w|EK
zWs<atmzh~)niX|kDX?p3Be84s1cT~Hs)P6rP=0vqN<Y22!@2V-KDn7fGFur)N5U3b
zmd~_Cl1T^S4BmR3uT>AR^VK3;A6HO*f8KiB@*yiptRR^gQ@m#td!{I)@%iJo)#?>;
zM#PD@X2se%A|H^c#|H|{zFNtq)ubdQ6DdT3<3+pzRS((~w4YNIbB}Q`C@0WqrOhNK
zO@DQ->L$e@=kh}ZO_;JL7p9u^ZC{;sdtY5lK@N=YD5@WLIPq*!gC||l%VypAn~ElM
z<sci4?e6Yx>YV5G-ur|UKmFK)66}KZocuQE(y}2ql@~jUWp($K*Tk~8r_ZVC=0N-y
zx%e>tLZP`Li-?ZwXW<sb1JJXiRIZzwwx}!V<U9GESxWZ8L*jdfX{qB6&hEX7$sOri
zRAY6l$MjIvCS4eSj>-iDWmgc0xmIR>%AwE3KaepX@HG)0rRzG-ZU$3(oMwmXnWp3@
z?07gnl~L*=kE+qgU)^ocP63+V0Z2`8jwz{eP{EXCQl0O)a_<-F)NFfHCw`qdIH7j$
zy8p(55C)%MUQHiSZcEE&9(-ga_D0~g8*VbDcjwOLtT7#rX}HXNHk7F8$er^|Y<fwo
zsv30f<9$^t!sjfZU-t)X3A<>K74N*7L~$@VyYz-CT9(Nprq$9?V^y{MAkOq&r#(o&
z)1-bwQ8_Eh@a2_8|6%(BmW;Jk#WlTMwaKOhZt1joEL^TM9_NZwOzc?uo-7Th5OZx&
z2jta6d*(h<4s`YgR?|MFnReMxHO?y0P~oe0^}M$l6u{<qJ4a^si&_fw3qNob5I;u8
zAGGzOoRLMDn9QM{{RSue8oZ959nu7T;o~=h{vmwr@bN<1z{Al5yye{1(nC@*#d*Ii
zGXZ_XIIgOl7dKa6%D$QjEqd>%vv)S#5YmE>PIL}efqq=JDclG#78+&%?R@bpU8L5u
zilIH{18?1QlQph<KI54AjAxCxm5^QyP>Qa{^)&KrLO}~{p!{5xUj=ipIOglt0?nD`
zTY)b>8FV^W#WxEAr8X@(2V`7lY@0$uiuw;-ShgKj53jltl4ZVs7G2Z|669k800x3T
zK8t?$fkO@e0Mzd5+~rYHmj~K`;Wm$;j^FDAinZUu+Zphhj=7k&e2epNSU15V&FN<V
z`B!8(c&OUK-ZCWlcDec5S%;4xXH8a8qZ@+*dhEfyZm>x%Z!mfKQOes4Xj)LzX~d>$
za@TDkDVFE)>70%UB{qFXxm8^OrA%Zri_I$9!)0d|9~SSUzQole9(33wTzmT46G#T{
zzC)w)@#H6Q4)JQ$&uDd05-Z^aHh4n<<bJ|Nnl_~D8{fDSfX;i21@=PX{N?gNXj+X$
zZb9*LsurB3sA_j~o+@f!jnmnDI-INxg1DH!4p;NzWh4~+%*d~f&yah{8Rr_+M1_21
zr)r#0iFt$dP;-TLaW?rPYW;14G8H?HBuz;?HrGYoa9h@v$if8uv?tp2Moj_WOE(D$
z05F4gom3Q5GQi*0<^ENERO$Y^)xQ_t{uIsY+TK4)aer9-7{XsmK&~t1e#!{?U+d@o
z-SXG1rt1=spF)HCmx7TWhJUvc^;0&f{(fKT-%);A{Xx2ZiX#1ga*Y3=V1Lc{Ph-G$
z682M`{RZq;O7>52ntp@xD@FUKIETN%`IWN$J<c_0`zbsO|Cyw}Qn<fIx@JH>#p*Xm
zKPlZmMM?P$%1?^-2g;8*d+qxNPrD|0KV|ecD1Srw{@Z-|YX><Q|1(uT3Ey8s`*p3n
yrhGr;A>*$E@Q3#wUHWx7xL!$r%3aRCSyvyZU|?RW5dnw*AanpA?z)U0@P7c)gApMB

literal 0
HcmV?d00001

diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg
deleted file mode 100644
index 69910c677459aae4664a08b1bbeb6aaed48721ad..0000000000000000000000000000000000000000
GIT binary patch
literal 0
HcmV?d00001

literal 11753
zcmeIYcT`hb6E+?}5RhIK&`_n9&^yvadPjN)0YXV23B3u@K|llnkuJSS?;uK15Ru+N
zdM_eH`fuUAzIyNLeSi1+{{CjIvsQA>%rmoRpG@|%_h_nMUL^+rZ~%Z%Jyp3hOzt)g
z003}#;Q#;tN0=iN?coFkJ2^SPAYe2M0q1drTX7@6C>V+x;RJ<SAs{Y}P&k^~1_^eu
zg+Wl7e*yf0r^#Y`6#%%rz5_zhU~YFu2MK9+M+Z7rC=vxjz$F<#+`J5QP&fo(1%um2
zGU(|la)~fVOW{c%tgT@XsMrrLT$lEA?v4&{l-M7SBpF<gaIx?4h{3^*P?Q)NBKB)w
zV!vvOeRuT79d`#9++LEw7L9fi<Kc00bK`d7=SCoHctE0}qCDT9GW@A(1^JJjlMB+}
zyOtG%2kHR596E{z#0}#4p?ewOzYXeA_t%h|kWds9j{ctKe~a;}&97Jp1mZtIU*_YF
zbbSw<kC#_~=a26{qDDEw9R97xOI<Dq!qEwghFLoNC8uCG%<<pCT<U%w>90}SAg!z%
z{uB2vlfuW#%g<xOV+BToxm;mTH^v{4|1pmLJzCMfjn)bYcKe?eAYPtJ9R|8T(8Ydv
zQTZ68e!P{J(<deIeG(x^==ZU3S%J||DL!5hKNpCX3&f`j;uGWL6B7`X;Q5W7gcU^W
zHwFR%Vjy7&o*xhY@Bp=fp<!?vE-M%0cTZB9x_lyPg2Fm~BLCO(znVcjAP!KJ6yM*p
z|GfQ&t_>6pMS{@?q?9Vm5((A#{@df>=N4j814A@)49(p|gy?>GWbTAS*g+v^9w?tE
zm=7et#b;>=wG!sz;uYcpSwV%kz@ox}d?FAo5o=x{A*(-K|Mc>Q*Z=&sQI{`13JrtM
zeNPp_@)Da1f^dPOB^mgI7<i;4{+!1@Gymh;e-OS;$R7m=2M6bSMFadI_y7O^1w~(`
z5p`LFri^A#b3(+v+vZQc>}B}SEh%k1=k=$Nm3>RAY)dy{HWS$Nkov*;lLe8}!}v&L
z(N|#_9VzNxV}R9rK7~VehXb+Q8l_jnJ+9ER?M4lV6pORGmfT}AN59BjO?Tb$UORUR
z`zpGhzL`aZ^;%CYK-)7^qHmziq14>%#l0ONQe|lJQ-GDRha<i}BB!cGsfMh4|EYoE
z5vub6J4LmraMb)jtxLG&tPnRh_vFH+wJqyiJJw9nM-e3O-m#qhnBLV))-KPz18;e6
z{&7`99!^l>gVMCR^%mhQ=-mfe-2Bx`U}C#un++Os67@%!yw)4j1ta>ms?RJtUCm>@
zeU4O|5`?^XibmDQ7ObL$zf|BRZ$Z~S>_=1U5ho^!DFc%QU1-%teKUw#6q26`Zcm7s
z=Lrz5<I?Hir}U^);q9w*#Sat<8WpR!0hb$IDe)R4eJ=L=S|o71J!sI)`E99FKkEVY
zIpvCF63lo0?Su7%%x_f>0|mQ$+_C%;uFq}s++SMxMqbE3DJQY#_95>6$~~w0JKWT+
zso(T8{aAJUHeUA<-z45bF9#~KK9VMW=jP*$znAgCiB!Gs)j}%=NxVGpDb^%OS-Oe7
zl4Wx=YlulWW9=&VfXdyDZtrc^gN{5REy0MJ75An|xA<cuDWB>E&DzTvTp+pLS!PC4
z7g~A2ZxFDu;B~yy51Uau_Fuc%ywRpxI>Bv|Rv1o^O~c2)VS42HB`h%7#$X($3|SAK
zE&Upm!UaiTq&O-y<d@yPd3vbkJQGsinc9zKq2G=@4FTTVN;Z_5kyONQYoO@7J;Su#
zTTi_Gtcw}X2hG{rb2U*N_L*iU3xfzsis!as@HsrAvE!b2U2oo@M38~Y{J<o;N$?%|
z6&_sk*Ni0yN{Y6HKrP_HjW))Pv1AK2p~uD_2vy^_+2uEa)nidyhi;fY(RQV1TFj9+
znaJ`*dl>9gx`{td_<Sd2cG8UP@CgoX-LUXlb<hQH2eUds7HGTa`ECdK?pkqmh){qa
z=s5a>6(B<%jwf2;4`FCV8b-bTvTttKIbt^qiVOQJC?Gt!QZ-3qnna!1$MI>%j{%!V
zLhcrMndtnis}0?@%ece4+jKG%2##W)JOkV0%z*h?5WTUaGVzG~llkir90hx_xpyd2
zZ!3v5MFai9E3ZiBUM*3z|CZmDbgOU}6kuD%a(pAvEPr)UO>j}m#XPg5*kEbwi5Xd4
z<<cIPC|I2#M_^MP+8w6&RSwLu<Ti%QYCU-9-{NR~lsa+m`Z%lHm)z!EuDA8kPiA{%
zHfM~w$QW41?8~$Bu;5P(UZqb8q!*FEYyF&j$0QDYpD;)bSq#%RzxsO0rImVCXEd(g
z-Am`Lu_g&rn$*$Kt0~D)rDin<?Zq!xorRN`kZjk5bF2w<w!+acQq${&tMYeNFbg=L
ziZTHcOcB&k_n%MUhRC#(7T~7vs<J(@I}^x`A4TxxslOavZF@!;c7fBT^wIVD07&2E
zn!F-L>Nm3sws@^)Zo5n|nHJcxBNq>?%!s)fa3NfblmUU=%H&t|m`9jmeIszP>_#@B
z^>VnJp&2DFmLN{1WJ*?5Gk3=2$==rH1aX(Y<E$WucI0z&(`iTI=Ghvs`63YBvSy#V
zk97{OP1~>*iWHJX^X4t_y|@c(?w+_J9n@T+QICPGgCpyjuD@t=G<iL@;V_K=K7G-J
zzQER0xrRfm5kHQH1pqiQ0s#M9L$3n>00;t(zFhLTJ34HdP9P`Vk@)l)sDdl<_s45L
z;A3O5KGSWes*1lRzS!kisO{k_Q>!_Ob>%8R0p`aLebnNu?1@c<aTQmVl(}1AgOn|^
z!G=25OC7?UGh;T*4MCGFD8$`a#v8`R!U{_`)+!e<@ICuhVaJg`<>#3k3|_#p@m(u^
zCZk<o_B?JU>a`f#NA}ga@~)>9gIt+&oX!a{&ibA<FiNJ!Lk|p2R1fq_*}59s%j<Nv
zj1pQMMTZPm4jiTmS0z;zORCZu*7G}LsqSt*4_VcnpJ32`ORi~d4ZrE=;Y_3SetWsg
z<$<+LeY06<mEcgncLt=>8O`Q+>%geVA-+?)`*4qrP~IjeO0Y<{zB?VAd(47v;ASpd
z)Qiyw?%<k|(XEhRy^94mU|OmSt`}-Lt$(2Uu_U?uc4CvgaM}wS?YcDf!L(HN!R>MG
zsa;PEZ^tu9oW<@Fx8%mI1f3Jodda*-iLN^{*4a~48UvLsdN*_*nbxb|za1>>y|uZY
zcPtj&MYo;jm9>^i<Ha?%ilnkl4{`aV`H1Gp?j{}SU2-q{P_r=O#Wyn(d(ty4C+!V{
z&$bjr#fpj)m9^^MV<%bOY=4>ssP~E;R?B>!PlvuH$3pe~6_s<&vdpt3C{4PPC&@#z
zdl7vub&-cUi7`S25tD~*1p(r^*TgXSv=EsVXOH^})#t-j9hh@o9?RX)&NsfUNboA-
zddf3(Iz8ba4)e+?K~;k=^Y#o3-rAl32~q~(XVx}Nl%5pKcyqzl<L7RIn5hNb80CbD
z2G3JmicYZD&8OAv)g$u;iE4}|EvZUbZR<GPffpIqli1hzb<^u}KcaB8xSxl_gro7A
znm(6w=~|WKcCl-lG!_?xc`FQUzgb1K_rB6L$1%Mj=;oAP&~3NGjiE4usDF1YCt|Xv
zgt4^lMO~e{KrC>u>s};mrpG56RwJav|C;q(H!CZ#F6|1ph98jyekYr54+8&4N#>1k
z=zaVnvzN{bZY3MT6g&P@C7vlueZc(C$7>yZ7<8wOdU0CrHO20XjXOei-8jJpn)bLB
zLkg)SL+f-#CUOh1!rV-%p4W0gpWY2Tyfam(DQ*xefScnF^_<&|z>1La2rgzcmr%L!
zytOeK$$R(=e(n_`7qTNzQO0aS?I!qe#;v>XA(~-NH7|=7tJWJ7t<^0+1_!U|Eg$O{
zJ)5Hkhp-a9DMxYKQa0P~58Wa${W=!>M&SM!wNSAg>jf=g!fRUud6%~!zE%o-JU*|f
zqO!fkph$$h+MOvPb5}p_fx;P#QWK>`mwI+A93mRU!`mFoT|}Y)KEB|Um!L|7ZDZp?
zO{$1Ulh)^k_NxA-m*E`CUA<bQ-V7bNX`gjIN6bi(oD4ip_G9?S0wvsmV0<u_edzbN
zdy>FQOUll0+jzeD#m-po*$PP7oK`pxh&@1tlf)s{kDb@lPe8KAm2SH=eZ38T>-HI8
z>Q2a6*TIQtF!9Z*vTw%3Ggld~9d&5(>v2dEZ9QWU>v^o6)DLt$V@U3Pe)1w*!_9N&
zrR3Bq!!f4OYbFk_=Fsi}V|jSF2CKg9+~;aajYF56$!|x1a|b$aTC1Ac#e`%F;I%Bv
z!I`b0%FqJAZA&c6whctQO+>CFWpBzGHHl5Hb`&o_T16sFB5l!EhnB)+aRa<V$YB~J
zQj0rR|Jtm0PJ~s9nS3O}C;QkiOt-AnspNFK<pKs;7wDH!9}gAP+c5KgS)8G-XPnb|
zcAdP-eCwG3Y!dK_U0z1RJ1;`JB2~Pazkx9C&WZjUpKI>nYkvuh5C)W_tpLt|gza4s
z$-tF9;y2Wy%7Gsk8LZNnWH)SaNO~lc1B<fg<uyowz7pQ*ak-mW^j4{?8sbXwD*_(W
zG)j*`eICe9k&N_?A5U+7HgbwTcH0c`oH?8;Hd!KRXc-F7*mvNTns*Iyu2^Cgj#&@N
z&%{U8Ihu6);>o&EL?}n<+CD$f$NH8<IRdA|r-_^B+Vs>#_-x8ChE0h(C*n=_>pdv8
zsRo1UrW_Uouj|&D(sH1L*$M{kdKlG1A2#cRDJBKo8bIT35O<l;=ToS#ji`1SGbmMX
zTErRjoH7lpd*Q~~JKt*$5!J89?>bd?<y1F5O?*T9?k+aNDBs%1c!WYDo`n$&GnAfm
zSSXO1f42BYb4)i)?4ZXbLZF{p*K$)`U<HSMHrXHqaln%$q4D*jwgm!G@JwjkZ?oTQ
zsM8c16jSJIhda=2)mMUB&1$u4G3>>(HEY_t+|%HH9(2zA)gy+WH<pf)@$i03@hPv@
z5m?63B8g6DS>q{PLPpSYlUjVoZ>_awD|^~l@^e9(J&(I%GS@oErJLS`D+pM`ikL3I
z8%AFoYu&)A`=Ikk=kaLH`#S%dQ&h*@uV}ni2lVkmcZ63?y_ks(&`Eb3grW5Kg`<)N
z=MHoVz>3!sl;w>3>9@xE1`bC*g^9wIYBss$0?Z&EBb~)jHE9eC^T7_I8UAQKxRoQ#
z`yzL)UC3n2_L^ET(r)n6v{2-mZEm@!>AR$wdmZkA@U0EBq2vbd?U$;#kbvoXiA3nA
zL#DpK>1!}1u9k%9b&LkggGXpPPwW|Eog3D^CIjGoFB>TNZnY|ZHVam4z<uw+H(4r`
zCSj%tlE}mASOfe)(ytHeZnusuXho_Kppwh{J8rF$6gh?yuxbsnNjVN%CyG#m^5sM`
z0(*)iD-J%4^V>_mMIHFCSY|~|eS91GRq*3mvGP(E>S{@i?d6c-n{#NKazVI=xt)SE
z?SV&>kikjac<<1;z>;mqGc(7zd-sKJ9q(%&OZX?V`Q+>+_1CaA)ek%5t4$eYMbCeu
ze_NCA&?%)SSNt9WXN^X*?D+l*mYuHm1GnZ|2U3aM-C5k1%y!;g-JyLGyfaokg!PIj
zjcaxS?GZmMWIMdAuykktqf2K>v|-)1jA=qj>Yibz;mN%mK_65n4_`K~e}AXgE6vv=
zy)c*3$^c!YC*5=EVi|8k<e5A5dnoKBCf>-6RNO)ZBKu%b!<LDUoH?SBS5v54ujLGx
zo3Ix9`Jj>QTIFPf2QgOl?U}E>90--sYSd!+-;1@Ao_VrltH%<(H(Z>%`)SR~)@=%P
zJUuiWU3bVwv=&)%0nQn-EXUdRPcomy_~Na({0R?KO*fyYN}}E}$~@<B)7^}Jh5DmC
z_SfgFFdj@y1etb%YjTPZqSwBOAJN+=`SqY%x(M~KVtwpJAMM+7JR8VY=afekE^_A;
zxW_Krn;*Yc)=l^B`<5J<cStzC=vV9M7}}*iLS8UWdYjl>rrPGdv{<&rL`LQ9H>;Kx
zL?h<x0}g=a#^Q0R74=>Zb+^2u9k1Y}jxW;t$q#`9twkRx2U?Fh6s_H$cFmK?54)+_
zeIvK298)li@2S>XSN!mHTCB0>P*K`&lJ=|bMoFt|4eeArpvAA)ll)3J3FPw^wHylW
zeBcn(169O8EGfoi$k%Y4iCI*uaNgWN-B_d5!nRAK=N@Fk!C%UwwR_S-k`>zeumf9h
zvYZ%v_sdRH(qjOQJV7w0HC>d4s%Uu%_^v#;S$`Pr2f^e+jo6)|cWq1Wt_po2e*JMQ
z8BccjQ4B0vG<-mMSujU6E@Fgd+ZKzM3dqPkO`;)Y^o6_)>p?HJKa{#Xt1XEB6Sf=Y
zea@{!_2_Eao{T%W0g9X-+YKTB*|vcjOYCrd42J+Olb|pqe|v{?_RgzF&=Ux;yc(HS
z1Y@fjzLllRhb7ALS60IEd6+G7_IDL|<FX?S$^D-O?B2CzOeS97#jX!HVCCEFO2vfX
z(l9^?V{TMNb6F)v#gW|^9(_2e8Xh|P&NY<LHyudxAo#PIN<D5HenKmh%;*Xc?+)+^
zk4RJyPK7g{gnYmOq+rlBy=QAhP|QW&JAR_?Bg2--WMIs|T6WUNJEBn`wrwudu)%`8
z8?~+}iK2MU-Q;2MB}pN|Igze#Nt09ifzHnqla*35*K|-jksuM9!m$)O<b%1}s;`Me
z@sE<WfjT{-#LquKmO;x0QWA_;mt=6-NO<jW{M(hVktls17Qowv^SklRx_8eBvn;uq
zO{tEw_N9{~YmdWA2gkjW6Ka`Sa#gl{A`+UH{a!YHlfGmXg*i&Lx9|age&i2U@nf%f
z@t&bYDmDOcdEEd20J^p=j+SsR%mKxN{_&C932qansjftTM~w#n00@-t$!h}ufGPms
z3IGQK004N(q(=e(*idD88C~}mn^Q1kvLPTy%OrlPswqdrKD?q?fI^!bTuZk!`vPbh
z6$!1H$?ar%iD>b=;I?>OlLZ9i<`9wk#H>7$2QnSg^<4{-y0<5j`Qw)4^z`%_Sod+m
zRC|H(ZnSxX6t-YU-D&-P@i1SHaeem)_sC-@?ENUHpyUGd&2B<g^zA7v#5CVv&z@HU
zu}{njBnK$%i$<KDpPigXTj?CT%T$C-lPgly()y_CMV+n02va4NpB}A3OawU^#YPu*
zzxwXz?tv`d3uvBM)P^Lu#uH^kTPf2HjV5H7sWDBR9Jyb(Uu>VPj~Jki=BzSSAfMh4
zgf8(CF>AAvv0WG&Xusej_i@Nhl@6x-cC=sdLbjl^A)XwP>wl0D?c04QWh6Ht9uyZw
z?x!!H;@g+A*9XZm9muFswI8<?^TxXz|G1E3$xw=HeM4=o*i2zGBrqjz_nBS9OQUVt
zhVZ^lUqy3w*H82J_sWW=8DEm=*xr^D50^SG8}3jNahh(R8ZgT7uze|lcJ1r*F2-3(
zQGFlMN0=tNS9UL@J(l0GsarUgcp}$esDY+`O6C^z=SB|WJIHUYg;Gm2)OA~%#e`DQ
z(uMwMySs<X(N3a1AA~)zONNbdCZ#r;wq|#z>-WAyJ25odPdf>1w|J3T9V5n;p@3tN
zC(}=Er&#A`K=@Z6QIf(mN;THO%~f5DgPq;OVtztirw5IBIzUGcGOO7cpR|bE`VyE$
z(GZ9FzE^XS>sg&DUZ8=hEmVYvev5|B9?gBL0~L2}Ptg>qdwU>TTfC$)l(?ttMdjX;
zqH=m<pP-5YkkKgNL5qL~zG<4zw+CaZdm#88-@2Jxw>fK+EO0=N?EUhxjr$1pHqlpG
zDDA$E-}H%!sr@eN9%w=`Cq(Obw0H4Nv@CFdi*`!KR8TZU>Ht;uKI>Dbd^bxpM~Bu5
z<QkP}h3?*$5k-wJ#pcUpf<C8163#8d?fVANS)Y`k5%yScVx^4e`|GHO!_<R?omwkd
z*@u<^1|dhc`IQzod|q7K$}=u$;Fwt7@F5Rrwx3XW<dX}_a`l(uc0T?5%$a<7d!E$q
zOLDQ!^N-U?>mFm`q`+;Uc}ufCFoP}2cLbK@>I9s^B$^`VAGT4d4lX{m*<U(1kq$#h
zI1WawW}UxS&HTD>J)v-<PT*Qjc$P=agyhDwk_*D?@XOk{oz$SI+HK>;w=*kBMQ*?e
zu3Nl*oTn8+%c)BXWv8j;o&fRFO#ad8=tdAo4ppq{Q}%V?OU%%H$3-c9qRhghB^Iw{
zw++-Vn_A(%mh`ew1C9OEMV^}92>(3mv%|irI>Y18I~N_BTP=qdbN<fcg&%+oXR`%|
z$<s$%?}6ed-Dd}7y+C-4#?JbdG!;=)mdNS1)ja*1a{Nk%wQEMHv`WtBQg#`UkA3H}
zqEPsvUce!n8)(FK)Z{MP%cFS>@sFT|4IlC^@rzT;1|I#%qIOg(S$xi~t0#y(a=Q0G
zQ;qU(LR6&22u=)TX}JzJ=7-o`dZ;JncpYecFzrs1FoI7gfsI`V+_oT@o{O0;OOg-!
zCJWw`Qjsf_CLe;IqB4>XL0zdTu3x3@L^Z{C$0Q$WwaGgqMb-NOgI2-*N_!xA;jG&u
zorDG!lkew`^G0owR>FJ|d2_rhf!0y0w?{(W&of7_KT7zA%JrLK_Wy4EDC_@MnK&i7
zc~#xHXf(?8@<>5hL0!H?)*|4q$DO|%HUNMC06-1_w}#pLd18a&<AFJXZJ;O~Ss2<8
z?1bX^XJhK$7&{@HT$~)CaF_o>J2V2}-~@(49sUpPF3)3JP?lii|FEO|U7SdS4HAm_
z|FOLM%l~_NE+hTl{(fh5nUbGYzdQJ0`He3y1Oj#V<G70l;(|o}xuJ;(xLmbd&g13s
z;!^&3l71JAwF3f-hW<#|?}E8p$e_?34$wc&pp1K>=4(lsm)Y>Sgzgbm`fjAc#0Q#2
zH!RDpxMH_{+^CaLiEF=gpPaQ|D%Ic}eF*I?rN^U2)tE-5aOqe!<MPGG2Cshc*OqMs
zq=izH(&b&xg=Zud9FQ9F3Leh$K?ARoV;GijxrT+Od4RrmN2Y~qT0Gtk#C2FD?bLo$
zrq^PwiPR<I<u1r3nRc&YeMW0gX+$Z)GNj$1ViubTGO8a}k&M6D$3tUZ5^rj+e0#wV
zl08w@UigLI9y%gM;?6&8xQdZe;?dPfEv<{6P1`QPQ})u!OoG?QyYUMjTX*(0LMVCF
zT4>R7GbEz@%}D?7Z0!(-eQTvY=}91o;Dg1GflmlDis6glMO?|cw$?HSgQDCcW+}rV
z?HD{=I+OT>d9#Ia^zOQ^q*%k&2qtgmP0FmVsvK@Q&}Wev@ek(4mkg~j*S$7durkhP
zCemCzn0L!iY*U1~W$BA6ypOBxdqokZdf3(#_HJ2pi6={AebcmhGdS#FPJ{DVYanaq
z2$9~CyO>R%t`W#@>2=jOFULKOqDEhfy-I}^*t>s7rx%3b$NPrEJSO(~7)ERqi)kXW
z9UJ3QQJNflW=Gu-24zpZ)|4coy(`I7Q#$2GUF$EfJITOh2{_M<G?HwY6TG6S1N}?U
z<D1v>%fwmR>Lt6h&8Wg2T*IXEU=2&ZADAR$>^l8vHn8CNdnqTW(+!TB+RFsI1(Lfj
zAVRzFWd0%A>>5N|&8v}iC5Ai4+g&L%+s5=F@>kj_b$KtINKd-AG9}8lT2UrCTRf8p
z1d$h%apFmNH2at{V_n?S3@H5IvpVJA>PCg-HuF{_e{jb7_=(Dy6P4-A6vUZ|YfA$1
z>{Ef2dqm`sgC#!84i#_GDPf!+LU0qzeUFpISQEGPosWDZxxIq@?A-oH6diKUmf1OD
zivmpS?Zci4w3QhTjRncDUMExH<9@hf1rU!O`ivXx2iE>vIqzDURy_SO3!RhPDo~3z
z-*~;MkeVRBT1If5I6<_8EOzesSoi0)$m|=V&R+AySls;7UXM$B6tf?uurmcMXyx0N
zg^A8PnSWao)AZZ;zy>XfZ)5XFG4psD$0NMM-ll@$XBE-z70<oT`6{)DKb|swD%6R~
zL(h_eSPe(GATj>tL6Ok|s<I+M<`uUYcZwbr1xZChVfFz10E=1|{|jBNhfnh6@wh!$
zmvI|YahVJrvo1Q>=P1aM3m4K<`bHq-XDW3*u=fXGsa{LpEf2^$oqc8GLNOv!>7qbv
zB1+aQ<B5!TNiNvOyVh!5=NqA7b~cOo$xP5gN$6>sN7Qxx(XiJmS%Pd28E1JZ@0Vt2
zY2TD1Z4J&0E=|s7KgPWqwv;^x!KYfME(NoK?t_TS$Uk?xNgt@;D$xe-pFiYJ+zr)u
zY*|&z^q$DnenI@&ErLZzmW4!>3OvQpDy>}6vI%~5rfD6eW=eU(<Ho#Cq1cLFA&6F{
zu%AYbqi4mJym&=pyU}#Z<-p@9dMig*KHHt4mEp_BT#OKwS;-fDZKzeS9GRMGc$|7!
zu&7L{L9KsH@Ic;TMU+HvN**3X#hDo1E4AoE4p>^*T#--!m~w!$$`SMlr>izSt+pNk
zAn6l)*;neq467eH$!rTN=pfngM>0p2#PLU2v}fxkOiA5j%pbh6as8^PO;j7_-C!~w
z`dnO1T^r(7Xf^|>B)V?}ucqlulU}_~R)iCFC!ScWTnY7~1~L9FTRncog*Kd_TB-KE
zEE0M>uPOJ7;}i3VK$O|s!i(znF#j87{x40}4Yo*XG>T6ek8bPloC%pbFQv#e1Ych^
zIF9`cjM&4c&Fp<GlNM^Lkbawq0EdStW2~JRPc5}YByfpJe&tw!0qp_`%v?QHVAxx;
z#OX0(k}Bm1>Ak+Vx*kC<;FlrMR7)qz^ECJEXG>z!Ge43l#%hXWX<heQj?Z6DYHk+k
zz0L`L#T<Ccfjx99FJHSJ;^=P~;<C=j*kLAoIGCpxwYC(hb*l}16&^)jn*ID$@c``;
zS9$Azk#+pBV58QE)bn>Wnsyc4uCj0JCvZWKo(%>AGC_9%=A<uO&P~|qfvfcUM@?lb
zLfaKLHJEgZaPUTD-BL`7JUzKCjylcH&w5)V6-wJJX7OAUjbXw#EX|B~Cn;0|-N*S+
zumF~AXRQ<mxM?i~Kd~hTkPvxSA@|DrZv13#q>g;@^*5}_UdWJ{!)*1mfGM4Zvqf}}
zI{MMKH3QOag80`BOH`55Ihq5g_RO;)z^zIQsTza;K`FM8lK**WuHfML4)SIG-m$yR
z`;C&;)cy~elXbS#itef+T#J(f;cIceaYCOih((F(>~bX4S8|bW1rGMqZ*JC)t7ya=
zTA%nnOW;U#NE;ZKcVqD~YsSEk6lWh&R%5&Aa9`RKE$7DhwNSTa+sid^X?kxF+oMCF
z+>6KoKEgPg-Y**1@<~3mxA(pkh4wdE5W#skNo`Aw#F#V4mnsR<eFG`K%%iloVx#G$
zg5xUPiyo#^n|Ck!Rz7U;;f`>{#z@3~Q@W)CmgKsZS>LGX-q?c>WksjVt+&0soGj@z
zwjZ<TMY5bE6qg+%v9oM43YIbRjoXExhv5|q-Yd7m^qUP=mLv&?*}+dc(;{ruo%AAd
zM@vH#mKE-IHxm(hZemO6I1N{4py~GN)BA_BAtS77^8ml?0fm!1sbcN4%8on|0g+N%
zOZV$oCN}Zsr$RfP=%|;|uR&$P4ut8i4gq$G``(nbdHkaScd~-mc8dK1Fj!4KO)0Ie
zshIfZM?{mgJ0Y9Zx-eraFC+l@RF#RfkFMd?zWLHwr3ow@&o**?lBe^<ihhNNJAq=M
zHdc(erh!O@o>lPWad(5wAxe?V<)m-k-+Ouc#x0$FgSQ2sg}l!jp}~s)g(9Qwndr9<
zEg$LKaMNG)`r0=T#o>BX*KM|}qq@LZ(t_zhq|$N$?4LZRx?Ck)>C)b)z3d$1|FKH?
z-Fk-t005{d=*n;@t1I$2g5fZ0DC+y!aj{M-{4E`R(;)}LmVa?BF7qb1zd8K`p!k9u
z7av1M&`+8)-!Ug&Cu`pta?)fYIk3_DQJ*FBy}QF0haZ?C{UD_^1DX~Ref)USExChD
zK$0moA)OtSSOV)t%CGA2DQ6;Mm|&}z7RxTK51IT9x|3F)b6s^9gX>Iv^Mqt@e|2tj
zIUE}oV->4boy4q@6kmxffZ>1QqX-Z*(t?q(Y<%NP<Z;=hFK`kN1C=R;U}`rOxrZct
zR<&d=#ZbGe>wQn-+VE}I`2JXJ2*lMqFH$Xlo1REyk{+Z^K$mmO{>&}9=_c}pqpER6
zCH8gZeXSMR`I+Ri==Iiy`!`u|C1^?#a5&C#``%=23D1oHr+hFklUe{+>?FMc08C?E
zCiMyiIpCk$Gyf_-nl=A!_4gjkpQ3fy;`wJs<`1hMQ}}B;z-8mqPwBt<*EXuZTmHJ`
zciArRQz-HN(me3P@Sk=Be~JU;KkpCz9p$Ih9~|tb+`aYRl;gh`*<UmM(-`obll>H*
z-+=wf(Ecq>)o*ZqWo-WzXY)5WzcRRg#<}EhKZW`Be<$g$jP9S2E(y_3G5-zHPloqz
zQR07t@{{rXf%4;>z4ZNqwq0_*pVIvslz(7=|LcAF>i`*m|DCFz{O_-!{aPz88Q@Pb
s1pdkge|Y~fq+iRyWhMP7_gMd-u4<}aVPC2d1Bd}aR{?+&wo9Y`4}A~OqW}N^


From 40610c7d484d077dc10726628c3adbe01edd91df Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 18 Oct 2013 10:38:51 +0200
Subject: [PATCH 09/47] Fixes, but approach needs proper design

---
 .../fw_pos_control_l1_main.cpp                | 66 ++++++++++++++-----
 src/modules/mavlink/waypoints.c               | 12 ++--
 2 files changed, 57 insertions(+), 21 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 219c6ffa6b..9ed74f0caf 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -158,6 +158,12 @@ private:
 	float		_launch_alt;
 	bool		_launch_valid;
 
+	/* land states */
+	/* not in non-abort mode for landing yet */
+	bool land_noreturn;
+	/* heading hold */
+	float target_bearing;
+
 	/* throttle and airspeed states */
 	float _airspeed_error;				///< airspeed error to setpoint in m/s
 	bool _airspeed_valid;				///< flag if a valid airspeed estimate exists
@@ -329,7 +335,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_airspeed_error(0.0f),
 	_airspeed_valid(false),
 	_groundspeed_undershoot(0.0f),
-	_global_pos_valid(false)
+	_global_pos_valid(false),
+	land_noreturn(false)
 {
 	_nav_capabilities.turn_distance = 0.0f;
 
@@ -635,9 +642,6 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
 
-	/* not in non-abort mode for landing yet */
-	bool land_noreturn = false;
-
 	/* AUTONOMOUS FLIGHT */
 
 	// XXX this should only execute if auto AND safety off (actuators active),
@@ -722,15 +726,26 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 				/* switch to heading hold for the last meters, continue heading hold after */
 
-				float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), next_wp.getX(), next_wp.getY());
-
-				if (wp_distance < 5.0f || land_noreturn) {
+				float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY());
+				//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
+				if (wp_distance < 15.0f || land_noreturn) {
 
 					/* heading hold, along the line connecting this and the last waypoint */
-					float target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
+					
+
+					// if (global_triplet.previous_valid) {
+					// 	target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY());
+					// } else {
+
+					if (!land_noreturn)
+						target_bearing = _att.yaw;
+					//}
+
+					warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw));
+
 					_l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed);
 
-					if (altitude_error > -20.0f)
+					if (altitude_error > -5.0f)
 						land_noreturn = true;
 
 				} else {
@@ -739,6 +754,12 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 					_l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed);
 				}
 
+				/* do not go down too early */
+				if (wp_distance > 50.0f) {
+					altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt;
+				}
+
+
 				_att_sp.roll_body = _l1_control.nav_roll();
 				_att_sp.yaw_body = _l1_control.nav_bearing();
 
@@ -748,15 +769,21 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 				float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1)
 				float land_pitch_min = math::radians(5.0f);
 				float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
-				float airspeed_land = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
+				float airspeed_land = _parameters.airspeed_min;
+				float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f;
 
-				if (altitude_error > -5.0f) {
+				if (altitude_error > -4.0f) {
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min),
+					/* land with minimal speed */
+
+					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
+					_tecs.set_speed_weight(2.0f);
+
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
 								    _airspeed.indicated_airspeed_m_s, eas2tas,
 								    false, flare_angle_rad,
 								    0.0f, _parameters.throttle_max, throttle_land,
-								    land_pitch_min, math::radians(_parameters.pitch_limit_max));
+								    math::radians(-10.0f), math::radians(15.0f));
 
 					/* kill the throttle if param requests it */
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
@@ -764,9 +791,11 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 					/* limit roll motion to prevent wings from touching the ground first */
 					_att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f));
 
-				} else if (altitude_error > -20.0f) {
+				} else if (wp_distance < 60.0f && altitude_error > -20.0f) {
 
-					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land),
+					/* minimize speed to approach speed */
+
+					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach),
 								    _airspeed.indicated_airspeed_m_s, eas2tas,
 								    false, flare_angle_rad,
 								    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -774,6 +803,8 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 
 				} else {
 
+					/* normal cruise speed */
+
 					_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim),
 								    _airspeed.indicated_airspeed_m_s, eas2tas,
 								    false, math::radians(_parameters.pitch_limit_min),
@@ -866,6 +897,11 @@ FixedwingPositionControl::control_position(const math::Vector2f &current_positio
 			}
 		}
 
+		/* reset land state */
+		if (global_triplet.current.nav_cmd != NAV_CMD_LAND) {
+			land_noreturn = false;
+		}
+
 		if (was_circle_mode && !_l1_control.circle_mode()) {
 			/* just kicked out of loiter, reset roll integrals */
 			_att_sp.roll_reset_integral = true;
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index adaf814047..7e4a2688f6 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -398,13 +398,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 
 			if (time_elapsed) {
 
-				if (cur_wp->autocontinue) {
+				/* safeguard against invalid missions with last wp autocontinue on */
+				if (wpm->current_active_wp_id == wpm->size - 1) {
+					/* stop handling missions here */
+					cur_wp->autocontinue = false;
+				}
 
-					/* safeguard against invalid missions with last wp autocontinue on */
-					if (wpm->current_active_wp_id == wpm->size - 1) {
-						/* stop handling missions here */
-						cur_wp->autocontinue = false;
-					}
+				if (cur_wp->autocontinue) {
 
 					cur_wp->current = 0;
 

From 6d406968eaba0924fe48945fd8a71b6f8a0adc1b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 19 Oct 2013 11:11:15 +0200
Subject: [PATCH 10/47] Added hexrotor support

---
 ROMFS/px4fmu_common/init.d/12-13_hex | 95 ++++++++++++++++++++++++++++
 ROMFS/px4fmu_common/init.d/rcS       | 14 ++++
 2 files changed, 109 insertions(+)
 create mode 100644 ROMFS/px4fmu_common/init.d/12-13_hex

diff --git a/ROMFS/px4fmu_common/init.d/12-13_hex b/ROMFS/px4fmu_common/init.d/12-13_hex
new file mode 100644
index 0000000000..0f0bb05ced
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/12-13_hex
@@ -0,0 +1,95 @@
+#!nsh
+
+echo "[init] PX4FMU v1, v2 with or without IO on Hex"
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	param set SYS_AUTOCONFIG 0
+
+	param set MC_ATTRATE_D 0.004
+	param set MC_ATTRATE_I 0.0
+	param set MC_ATTRATE_P 0.12
+	param set MC_ATT_D 0.0
+	param set MC_ATT_I 0.0
+	param set MC_ATT_P 7.0
+	param set MC_YAWPOS_D 0.0
+	param set MC_YAWPOS_I 0.0
+	param set MC_YAWPOS_P 2.0
+	param set MC_YAWRATE_D 0.005
+	param set MC_YAWRATE_I 0.2
+	param set MC_YAWRATE_P 0.3
+	param set NAV_TAKEOFF_ALT 3.0
+	param set MPC_TILT_MAX 0.5
+	param set MPC_THR_MAX 0.7
+	param set MPC_THR_MIN 0.3
+	param set MPC_XY_D 0
+	param set MPC_XY_P 0.5
+	param set MPC_XY_VEL_D 0
+	param set MPC_XY_VEL_I 0
+	param set MPC_XY_VEL_MAX 3
+	param set MPC_XY_VEL_P 0.2
+	param set MPC_Z_D 0
+	param set MPC_Z_P 1
+	param set MPC_Z_VEL_D 0
+	param set MPC_Z_VEL_I 0.1
+	param set MPC_Z_VEL_MAX 2
+	param set MPC_Z_VEL_P 0.20
+
+	param save
+fi
+ 
+#
+# Force some key parameters to sane values
+# MAV_TYPE list: https://pixhawk.ethz.ch/mavlink/
+# 13 = hexarotor
+#
+param set MAV_TYPE 13
+
+set EXIT_ON_END no
+
+#
+# Start and configure PX4IO or FMU interface
+#
+if px4io detect
+then
+	# Start MAVLink (depends on orb)
+	mavlink start
+	usleep 5000
+
+	sh /etc/init.d/rc.io
+	# Set PWM values for DJI ESCs
+	px4io idle 900 900 900 900 900 900
+	px4io min 1200 1200 1200 1200 1200 1200
+	px4io max 1900 1900 1900 1900 1900 1900
+else
+	# Start MAVLink (on UART1 / ttyS0)
+	mavlink start -d /dev/ttyS0
+	usleep 5000
+	fmu mode_pwm
+	param set BAT_V_SCALING 0.004593
+	set EXIT_ON_END yes
+fi
+
+#
+# Load mixer
+#
+mixer load /dev/pwm_output $MIXER
+
+#
+# Set PWM output frequency to 400 Hz
+#
+pwm -u 400 -m 0xff
+
+#
+# Start common for all multirotors apps
+#
+sh /etc/init.d/rc.multirotor
+
+if [ $EXIT_ON_END == yes ]
+then
+	exit
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 93e76184d4..ced8750015 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -177,6 +177,20 @@ then
 		sh /etc/init.d/11_dji_f450
 		set MODE custom
 	fi
+
+	if param compare SYS_AUTOSTART 12
+	then
+		set MIXER /etc/mixers/FMU_hex_x.mix
+		sh /etc/init.d/12-13_hex
+		set MODE custom
+	fi
+
+	if param compare SYS_AUTOSTART 13
+	then
+		set MIXER /etc/mixers/FMU_hex_+.mix
+		sh /etc/init.d/12-13_hex
+		set MODE custom
+	fi
 	
 	if param compare SYS_AUTOSTART 15
 	then

From 233a068a7b6eb4851aa47b80a1852851bc851d73 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Tue, 15 Oct 2013 17:54:57 +0200
Subject: [PATCH 11/47] quad hil + rotor configuration startup script

---
 ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil | 110 ++++++++++++++++++
 ROMFS/px4fmu_common/init.d/rcS                |  40 ++++---
 2 files changed, 135 insertions(+), 15 deletions(-)
 create mode 100644 ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil

diff --git a/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
new file mode 100644
index 0000000000..1c85e04f2a
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1003_rc_quad_+.hil
@@ -0,0 +1,110 @@
+#!nsh
+#
+# USB HIL start
+#
+
+echo "[HIL] HILS quadrotor + starting.."
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+        # Set all params here, then disable autoconfig
+        param set SYS_AUTOCONFIG 0
+
+        param set MC_ATTRATE_D 0.0
+        param set MC_ATTRATE_I 0.0
+        param set MC_ATTRATE_P 0.05
+        param set MC_ATT_D 0.0
+        param set MC_ATT_I 0.0
+        param set MC_ATT_P 3.0
+        param set MC_YAWPOS_D 0.0
+        param set MC_YAWPOS_I 0.0
+        param set MC_YAWPOS_P 2.1
+        param set MC_YAWRATE_D 0.0
+        param set MC_YAWRATE_I 0.0
+        param set MC_YAWRATE_P 0.05
+        param set NAV_TAKEOFF_ALT 3.0
+        param set MPC_TILT_MAX 0.5
+        param set MPC_THR_MAX 0.5
+        param set MPC_THR_MIN 0.1
+        param set MPC_XY_D 0
+        param set MPC_XY_P 0.5
+        param set MPC_XY_VEL_D 0
+        param set MPC_XY_VEL_I 0
+        param set MPC_XY_VEL_MAX 3
+        param set MPC_XY_VEL_P 0.2
+        param set MPC_Z_D 0
+        param set MPC_Z_P 1
+        param set MPC_Z_VEL_D 0
+        param set MPC_Z_VEL_I 0.1
+        param set MPC_Z_VEL_MAX 2
+        param set MPC_Z_VEL_P 0.20
+
+        param save
+fi
+
+# Allow USB some time to come up
+sleep 1
+# Tell MAVLink that this link is "fast"
+mavlink start -b 230400 -d /dev/ttyACM0
+
+# Create a fake HIL /dev/pwm_output interface
+hil mode_pwm
+
+#
+# Force some key parameters to sane values
+# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
+#              see https://pixhawk.ethz.ch/mavlink/
+#
+param set MAV_TYPE 2
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+
+#
+# Check if we got an IO
+#
+if px4io start
+then
+	echo "IO started"
+else
+	fmu mode_serial
+	echo "FMU started"
+fi
+
+#
+# Start the sensors (depends on orb, px4io)
+#
+sh /etc/init.d/rc.sensors
+
+#
+# Start the attitude estimator (depends on orb)
+#
+att_pos_estimator_ekf start
+ 
+#
+# Load mixer and start controllers (depends on px4io)
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_quad_+.mix
+
+#
+# Start position estimator
+#
+position_estimator_inav start
+
+#
+# Start attitude control
+#
+multirotor_att_control start
+
+#
+# Start position control
+#
+multirotor_pos_control start
+
+echo "[HIL] setup done, running"
+
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index ced8750015..fd588017f9 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -110,23 +110,33 @@ then
 	then
 		sh /etc/init.d/1000_rc_fw.hil		
 		set MODE custom
-	else
-		if param compare SYS_AUTOSTART 1001
-		then
-			sh /etc/init.d/1001_rc_quad.hil
-			set MODE custom
-		else
-			if param compare SYS_AUTOSTART 1002
-			then
-				sh /etc/init.d/1002_rc_fw_state.hil
-				set MODE custom
-			else
-				# Try to get an USB console
-				nshterm /dev/ttyACM0 &
-			fi	
-		fi
 	fi
 	
+	if param compare SYS_AUTOSTART 1001
+	then
+		sh /etc/init.d/1001_rc_quad.hil
+		set MODE custom
+	fi
+	
+	if param compare SYS_AUTOSTART 1002
+	then
+		sh /etc/init.d/1002_rc_fw_state.hil
+		set MODE custom
+	fi
+	
+	if param compare SYS_AUTOSTART 1003
+	then
+		sh /etc/init.d/1003_rc_quad_+.hil
+		set MODE custom
+	fi
+	
+	if [ $MODE != custom ]
+	then
+		# Try to get an USB console
+		nshterm /dev/ttyACM0 &
+	fi
+
+	
 	#
 	# Upgrade PX4IO firmware
 	#

From 5cd675d8cce6a3c35eab1970cfb8668b0a6b24e5 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 19 Oct 2013 11:42:06 +0200
Subject: [PATCH 12/47] The mavlink baudrate was too high in the custom_io_esc
 startup script

---
 ROMFS/px4fmu_common/init.d/rc.custom_io_esc | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc
index e645d9d549..2bfaed76cb 100644
--- a/ROMFS/px4fmu_common/init.d/rc.custom_io_esc
+++ b/ROMFS/px4fmu_common/init.d/rc.custom_io_esc
@@ -58,7 +58,7 @@ usleep 10000
 if px4io detect
 then
 	# Start MAVLink (depends on orb)
-	mavlink start -d /dev/ttyS1 -b 115200
+	mavlink start -d /dev/ttyS1 -b 57600
 	usleep 5000
 
 	sh /etc/init.d/rc.io
@@ -78,7 +78,7 @@ then
 else
 	fmu mode_pwm
 	# Start MAVLink (on UART1 / ttyS0)
-	mavlink start -d /dev/ttyS1 -b 115200
+	mavlink start -d /dev/ttyS1 -b 57600
 	usleep 5000
 	param set BAT_V_SCALING 0.004593
 	set EXIT_ON_END yes

From dbb49c035bb85876b234fc057d9f244b43453a44 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 19 Oct 2013 15:19:42 +1100
Subject: [PATCH 13/47] rgbled: fixed detection of device on PX4v1

There is a serial EEPROM on the PX4IOv1 board that answers on I2C
address 0x55. We need some extra I2C transfers to ensure we are
talking to a real RGBLED device.
---
 src/drivers/rgbled/rgbled.cpp | 15 ++++++++++++++-
 1 file changed, 14 insertions(+), 1 deletion(-)

diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index ea87b37d94..34a45c7393 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -172,7 +172,20 @@ RGBLED::probe()
 	bool on, powersave;
 	uint8_t r, g, b;
 
-	ret = get(on, powersave, r, g, b);
+	/**
+	   this may look strange, but is needed. There is a serial
+	   EEPROM (Microchip-24aa01) on the PX4FMU-v1 that responds to
+	   a bunch of I2C addresses, including the 0x55 used by this
+	   LED device. So we need to do enough operations to be sure
+	   we are talking to the right device. These 3 operations seem
+	   to be enough, as the 3rd one consistently fails if no
+	   RGBLED is on the bus.
+	 */
+	if ((ret=get(on, powersave, r, g, b)) != OK ||
+	    (ret=send_led_enable(false) != OK) ||
+	    (ret=send_led_enable(false) != OK)) {
+		return ret;
+	}
 
 	return ret;
 }

From e3fe4437204b2aecdaa1131fcb4bb0c7751a43df Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 19 Oct 2013 15:21:02 +1100
Subject: [PATCH 14/47] rgbled: fixed getopt() handling

this allows the -a option to be used, for example
  rgbled -a 0x55 start
---
 src/drivers/rgbled/rgbled.cpp | 9 +++++++--
 1 file changed, 7 insertions(+), 2 deletions(-)

diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index 34a45c7393..d49211b7b8 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -574,7 +574,7 @@ rgbled_main(int argc, char *argv[])
 	int ch;
 
 	/* jump over start/off/etc and look at options first */
-	while ((ch = getopt(argc - 1, &argv[1], "a:b:")) != EOF) {
+	while ((ch = getopt(argc, argv, "a:b:")) != EOF) {
 		switch (ch) {
 		case 'a':
 			rgbledadr = strtol(optarg, NULL, 0);
@@ -590,7 +590,12 @@ rgbled_main(int argc, char *argv[])
 		}
 	}
 
-	const char *verb = argv[1];
+        if (optind >= argc) {
+            rgbled_usage();
+            exit(1);
+        }
+
+	const char *verb = argv[optind];
 
 	int fd;
 	int ret;

From 14e2464fabbae27f9655efdb3e5ee55479c469f7 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Sat, 19 Oct 2013 15:21:46 +1100
Subject: [PATCH 15/47] rgbled: don't try the same bus twice

on PX4v1 the external I2C bus is the same as the LED bus
---
 src/drivers/rgbled/rgbled.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/drivers/rgbled/rgbled.cpp b/src/drivers/rgbled/rgbled.cpp
index d49211b7b8..727c86e02a 100644
--- a/src/drivers/rgbled/rgbled.cpp
+++ b/src/drivers/rgbled/rgbled.cpp
@@ -616,6 +616,9 @@ rgbled_main(int argc, char *argv[])
 
 			if (g_rgbled == nullptr) {
 				// fall back to default bus
+				if (PX4_I2C_BUS_LED == PX4_I2C_BUS_EXPANSION) {
+					errx(1, "init failed");
+				}
 				i2cdevice = PX4_I2C_BUS_LED;
 			}
 		}

From 6a624ff753ad9ba6e7fb74783b6b8294a1c17957 Mon Sep 17 00:00:00 2001
From: Stefan Rado <px4@sradonia.net>
Date: Sat, 19 Oct 2013 23:04:36 +0200
Subject: [PATCH 16/47] Fix gyro calibration for rotated sensors. The
 calibration routine now uses the raw sensor values instead of the already
 rotated values.

---
 src/modules/commander/gyro_calibration.cpp | 194 +++++++++------------
 1 file changed, 86 insertions(+), 108 deletions(-)

diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 369e6da624..b6de5141f6 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -60,17 +60,7 @@ int do_gyro_calibration(int mavlink_fd)
 {
 	mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
 
-	const unsigned calibration_count = 5000;
-
-	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
-	struct sensor_combined_s raw;
-
-	unsigned calibration_counter = 0;
-	float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
-
-	/* set offsets to zero */
-	int fd = open(GYRO_DEVICE_PATH, 0);
-	struct gyro_scale gscale_null = {
+	struct gyro_scale gyro_scale = {
 		0.0f,
 		1.0f,
 		0.0f,
@@ -79,27 +69,40 @@ int do_gyro_calibration(int mavlink_fd)
 		1.0f,
 	};
 
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null))
-		warn("WARNING: failed to set scale / offsets for gyro");
+	/* subscribe to gyro sensor topic */
+	int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+	struct gyro_report gyro_report;
+
+	/* reset all offsets to zero and all scales to one */
+	int fd = open(GYRO_DEVICE_PATH, 0);
+
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
+		warn("WARNING: failed to reset scale / offsets for gyro");
 
 	close(fd);
 
+
+	/*** --- OFFSETS --- ***/
+
+	/* determine gyro mean values */
+	const unsigned calibration_count = 5000;
+	unsigned calibration_counter = 0;
 	unsigned poll_errcount = 0;
 
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
 		struct pollfd fds[1];
-		fds[0].fd = sub_sensor_combined;
+		fds[0].fd = sub_sensor_gyro;
 		fds[0].events = POLLIN;
 
 		int poll_ret = poll(fds, 1, 1000);
 
 		if (poll_ret > 0) {
-			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
-			gyro_offset[0] += raw.gyro_rad_s[0];
-			gyro_offset[1] += raw.gyro_rad_s[1];
-			gyro_offset[2] += raw.gyro_rad_s[2];
+			orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
+			gyro_scale.x_offset += gyro_report.x;
+			gyro_scale.y_offset += gyro_report.y;
+			gyro_scale.z_offset += gyro_report.z;
 			calibration_counter++;
 			if (calibration_counter % (calibration_count / 20) == 0)
 				mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count);
@@ -110,67 +113,49 @@ int do_gyro_calibration(int mavlink_fd)
 
 		if (poll_errcount > 1000) {
 			mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
-			close(sub_sensor_combined);
+			close(sub_sensor_gyro);
 			return ERROR;
 		}
 	}
 
-	gyro_offset[0] = gyro_offset[0] / calibration_count;
-	gyro_offset[1] = gyro_offset[1] / calibration_count;
-	gyro_offset[2] = gyro_offset[2] / calibration_count;
+	gyro_scale.x_offset /= calibration_count;
+	gyro_scale.y_offset /= calibration_count;
+	gyro_scale.z_offset /= calibration_count;
 
-
-	if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) {
-
-		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0]))
-			|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1]))
-			|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) {
-			mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!");
-		}
-
-		/* set offsets to actual value */
-		fd = open(GYRO_DEVICE_PATH, 0);
-		struct gyro_scale gscale = {
-			gyro_offset[0],
-			1.0f,
-			gyro_offset[1],
-			1.0f,
-			gyro_offset[2],
-			1.0f,
-		};
-
-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
-			warn("WARNING: failed to set scale / offsets for gyro");
-
-		close(fd);
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warnx("WARNING: auto-save of params to storage failed");
-			mavlink_log_critical(mavlink_fd, "gyro store failed");
-			close(sub_sensor_combined);
-			return ERROR;
-		}
-
-		tune_neutral();
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
-		close(sub_sensor_combined);
+	if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
+		mavlink_log_info(mavlink_fd, "gyro offset calibration FAILED (NaN)");
+		close(sub_sensor_gyro);
 		return ERROR;
 	}
 
+	/* beep on calibration end */
 	mavlink_log_info(mavlink_fd, "offset calibration done.");
+	tune_neutral();
+
+	/* set offset parameters to new values */
+	if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
+		|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
+		|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
+		mavlink_log_critical(mavlink_fd, "Setting gyro offset parameters failed!");
+	}
+
 
-#if 0
 	/*** --- SCALING --- ***/
+#if 0
+	/* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */
 
 	mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
 	warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
 
+	/* apply new offsets */
+	fd = open(GYRO_DEVICE_PATH, 0);
+
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
+		warn("WARNING: failed to apply new offsets for gyro");
+
+	close(fd);
+
+
 	unsigned rotations_count = 30;
 	float gyro_integral = 0.0f;
 	float baseline_integral = 0.0f;
@@ -246,52 +231,45 @@ int do_gyro_calibration(int mavlink_fd)
 	
 	warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
 	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
-#else
-	float gyro_scales[] = { 1.0f, 1.0f, 1.0f };
-#endif
-
-	
 
 
-	if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) {
-
-		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0]))
-			|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1]))
-			|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) {
-			mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!");
-		}
-
-		/* set offsets to actual value */
-		fd = open(GYRO_DEVICE_PATH, 0);
-		struct gyro_scale gscale = {
-			gyro_offset[0],
-			gyro_scales[0],
-			gyro_offset[1],
-			gyro_scales[1],
-			gyro_offset[2],
-			gyro_scales[2],
-		};
-
-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
-			warn("WARNING: failed to set scale / offsets for gyro");
-
-		close(fd);
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		mavlink_log_info(mavlink_fd, "gyro calibration done");
-
-		/* third beep by cal end routine */
-		close(sub_sensor_combined);
-		return OK;
-	} else {
-		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
-		close(sub_sensor_combined);
+	if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) {
+		mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)");
+		close(sub_sensor_gyro);
 		return ERROR;
 	}
+
+	/* beep on calibration end */
+	mavlink_log_info(mavlink_fd, "scale calibration done.");
+	tune_neutral();
+
+#endif
+
+	/* set scale parameters to new values */
+	if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
+		|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
+		|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
+		mavlink_log_critical(mavlink_fd, "Setting gyro scale parameters failed!");
+	}
+
+	/* apply new scaling and offsets */
+	fd = open(GYRO_DEVICE_PATH, 0);
+
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
+		warn("WARNING: failed to apply new scale for gyro");
+
+	close(fd);
+
+	/* auto-save to EEPROM */
+	int save_ret = param_save_default();
+
+	if (save_ret != 0) {
+		warnx("WARNING: auto-save of params to storage failed");
+		mavlink_log_critical(mavlink_fd, "gyro store failed");
+		close(sub_sensor_gyro);
+		return ERROR;
+	}
+
+	close(sub_sensor_gyro);
+	return OK;
 }

From 8cffd2b8a33ccb4d556a181d5a6e55111c1b1f53 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 20 Oct 2013 12:16:00 +0200
Subject: [PATCH 17/47] fix scaling (unit) of airspeed in HIL

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

diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 8243748dc8..c51a6de088 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -422,13 +422,13 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.accelerometer_counter = hil_counter;
 
 			/* differential pressure */
-			hil_sensors.differential_pressure_pa = imu.diff_pressure;
+			hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
 			hil_sensors.differential_pressure_counter = hil_counter;
 
 			/* airspeed from differential pressure, ambient pressure and temp */
 			struct airspeed_s airspeed;
 
-			float ias = calc_indicated_airspeed(imu.diff_pressure);
+			float ias = calc_indicated_airspeed(hil_sensors.differential_pressure_pa);
 			// XXX need to fix this
 			float tas = ias;
 

From ef6f1f6f808e49ff3aca68aa08001e37093b89ec Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sun, 20 Oct 2013 19:36:42 +0200
Subject: [PATCH 18/47] get_rot_matrix() moved to separate library, accel
 calibration of rotated board fixed

---
 makefiles/config_px4fmu-v1_default.mk         |  1 +
 makefiles/config_px4fmu-v2_default.mk         |  1 +
 src/lib/conversion/module.mk                  | 38 ++++++++
 src/lib/conversion/rotation.cpp               | 30 ++++++
 src/lib/conversion/rotation.h                 | 89 ++++++++++++++++++
 .../commander/accelerometer_calibration.cpp   | 45 ++++++---
 src/modules/commander/module.mk               |  1 -
 src/modules/sensors/sensors.cpp               | 93 +------------------
 8 files changed, 192 insertions(+), 106 deletions(-)
 create mode 100644 src/lib/conversion/module.mk
 create mode 100644 src/lib/conversion/rotation.cpp
 create mode 100644 src/lib/conversion/rotation.h

diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 46640f3c51..862abde92b 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -115,6 +115,7 @@ MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
 MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/conversion
 
 #
 # Demo apps
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index bb589cb9f3..71986c3a34 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -111,6 +111,7 @@ MODULES		+= lib/mathlib/math/filter
 MODULES		+= lib/ecl
 MODULES		+= lib/external_lgpl
 MODULES		+= lib/geo
+MODULES		+= lib/conversion
 
 #
 # Demo apps
diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk
new file mode 100644
index 0000000000..102711aaf7
--- /dev/null
+++ b/src/lib/conversion/module.mk
@@ -0,0 +1,38 @@
+############################################################################
+#
+#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Rotation library
+#
+
+SRCS		 = rotation.cpp
diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp
new file mode 100644
index 0000000000..b65226cf1c
--- /dev/null
+++ b/src/lib/conversion/rotation.cpp
@@ -0,0 +1,30 @@
+/*
+ * rotation.cpp
+ *
+ *  Created on: 20.10.2013
+ *      Author: ton
+ */
+
+#include "math.h"
+#include "rotation.h"
+
+__EXPORT void
+get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix)
+{
+	/* first set to zero */
+	rot_matrix->Matrix::zero(3, 3);
+
+	float roll  = M_DEG_TO_RAD_F * (float)rot_lookup[rot].roll;
+	float pitch = M_DEG_TO_RAD_F * (float)rot_lookup[rot].pitch;
+	float yaw   = M_DEG_TO_RAD_F * (float)rot_lookup[rot].yaw;
+
+	math::EulerAngles euler(roll, pitch, yaw);
+
+	math::Dcm R(euler);
+
+	for (int i = 0; i < 3; i++) {
+		for (int j = 0; j < 3; j++) {
+			(*rot_matrix)(i, j) = R(i, j);
+		}
+	}
+}
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
new file mode 100644
index 0000000000..97c6a09076
--- /dev/null
+++ b/src/lib/conversion/rotation.h
@@ -0,0 +1,89 @@
+/*
+ * rotation.h
+ *
+ *  Created on: 20.10.2013
+ *      Author: ton
+ */
+
+#ifndef ROTATION_H_
+#define ROTATION_H_
+
+#include <unistd.h>
+#include <mathlib/mathlib.h>
+
+/**
+ * Enum for board and external compass rotations.
+ * This enum maps from board attitude to airframe attitude.
+ */
+enum Rotation {
+	ROTATION_NONE                = 0,
+	ROTATION_YAW_45              = 1,
+	ROTATION_YAW_90              = 2,
+	ROTATION_YAW_135             = 3,
+	ROTATION_YAW_180             = 4,
+	ROTATION_YAW_225             = 5,
+	ROTATION_YAW_270             = 6,
+	ROTATION_YAW_315             = 7,
+	ROTATION_ROLL_180            = 8,
+	ROTATION_ROLL_180_YAW_45     = 9,
+	ROTATION_ROLL_180_YAW_90     = 10,
+	ROTATION_ROLL_180_YAW_135    = 11,
+	ROTATION_PITCH_180           = 12,
+	ROTATION_ROLL_180_YAW_225    = 13,
+	ROTATION_ROLL_180_YAW_270    = 14,
+	ROTATION_ROLL_180_YAW_315    = 15,
+	ROTATION_ROLL_90             = 16,
+	ROTATION_ROLL_90_YAW_45      = 17,
+	ROTATION_ROLL_90_YAW_90      = 18,
+	ROTATION_ROLL_90_YAW_135     = 19,
+	ROTATION_ROLL_270            = 20,
+	ROTATION_ROLL_270_YAW_45     = 21,
+	ROTATION_ROLL_270_YAW_90     = 22,
+	ROTATION_ROLL_270_YAW_135    = 23,
+	ROTATION_PITCH_90            = 24,
+	ROTATION_PITCH_270           = 25,
+	ROTATION_MAX
+};
+
+typedef struct {
+	uint16_t roll;
+	uint16_t pitch;
+	uint16_t yaw;
+} rot_lookup_t;
+
+const rot_lookup_t rot_lookup[] = {
+	{  0,   0,   0 },
+	{  0,   0,  45 },
+	{  0,   0,  90 },
+	{  0,   0, 135 },
+	{  0,   0, 180 },
+	{  0,   0, 225 },
+	{  0,   0, 270 },
+	{  0,   0, 315 },
+	{180,   0,   0 },
+	{180,   0,  45 },
+	{180,   0,  90 },
+	{180,   0, 135 },
+	{  0, 180,   0 },
+	{180,   0, 225 },
+	{180,   0, 270 },
+	{180,   0, 315 },
+	{ 90,   0,   0 },
+	{ 90,   0,  45 },
+	{ 90,   0,  90 },
+	{ 90,   0, 135 },
+	{270,   0,   0 },
+	{270,   0,  45 },
+	{270,   0,  90 },
+	{270,   0, 135 },
+	{  0,  90,   0 },
+	{  0, 270,   0 }
+};
+
+/**
+ * Get the rotation matrix
+ */
+__EXPORT void
+get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
+
+#endif /* ROTATION_H_ */
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index cfa7d9e8a1..c9bfedbda0 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -112,11 +112,13 @@
 #include <fcntl.h>
 #include <sys/prctl.h>
 #include <math.h>
+#include <mathlib/mathlib.h>
 #include <string.h>
 #include <drivers/drv_hrt.h>
 #include <uORB/topics/sensor_combined.h>
 #include <drivers/drv_accel.h>
 #include <geo/geo.h>
+#include <conversion/rotation.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <mavlink/mavlink_log.h>
@@ -144,24 +146,41 @@ int do_accel_calibration(int mavlink_fd) {
 	int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
 
 	if (res == OK) {
-		/* measurements complete successfully, set parameters */
-		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs[0]))
-			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs[1]))
-			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs[2]))
-			|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale[0]))
-			|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale[1]))
-			|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale[2]))) {
+		/* measurements complete successfully, rotate calibration values */
+		param_t board_rotation_h = param_find("SENS_BOARD_ROT");
+		enum Rotation board_rotation_id;
+		param_get(board_rotation_h, &(board_rotation_id));
+		math::Matrix board_rotation(3, 3);
+		get_rot_matrix(board_rotation_id, &board_rotation);
+		board_rotation = board_rotation.transpose();
+		math::Vector3 vect(3);
+		vect(0) = accel_offs[0];
+		vect(1) = accel_offs[1];
+		vect(2) = accel_offs[2];
+		math::Vector3 accel_offs_rotated = board_rotation * vect;
+		vect(0) = accel_scale[0];
+		vect(1) = accel_scale[1];
+		vect(2) = accel_scale[2];
+		math::Vector3 accel_scale_rotated = board_rotation * vect;
+
+		/* set parameters */
+		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0)))
+			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1)))
+			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2)))
+			|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale_rotated(0)))
+			|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale_rotated(1)))
+			|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale_rotated(2)))) {
 			mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
 		}
 
 		int fd = open(ACCEL_DEVICE_PATH, 0);
 		struct accel_scale ascale = {
-			accel_offs[0],
-			accel_scale[0],
-			accel_offs[1],
-			accel_scale[1],
-			accel_offs[2],
-			accel_scale[2],
+			accel_offs_rotated(0),
+			accel_scale_rotated(0),
+			accel_offs_rotated(1),
+			accel_scale_rotated(1),
+			accel_offs_rotated(2),
+			accel_scale_rotated(2),
 		};
 
 		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk
index 91d75121eb..554dfcb080 100644
--- a/src/modules/commander/module.mk
+++ b/src/modules/commander/module.mk
@@ -47,4 +47,3 @@ SRCS		 	= commander.cpp \
 			baro_calibration.cpp \
 			rc_calibration.cpp \
 			airspeed_calibration.cpp
-
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 3fcacaf8f9..1b79de8fd2 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -67,6 +67,7 @@
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
+#include <conversion/rotation.h>
 
 #include <systemlib/airspeed.h>
 
@@ -135,75 +136,6 @@
 
 #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.
- * This enum maps from board attitude to airframe attitude.
- */
-enum Rotation {
-	ROTATION_NONE                = 0,
-	ROTATION_YAW_45              = 1,
-	ROTATION_YAW_90              = 2,
-	ROTATION_YAW_135             = 3,
-	ROTATION_YAW_180             = 4,
-	ROTATION_YAW_225             = 5,
-	ROTATION_YAW_270             = 6,
-	ROTATION_YAW_315             = 7,
-	ROTATION_ROLL_180            = 8,
-	ROTATION_ROLL_180_YAW_45     = 9,
-	ROTATION_ROLL_180_YAW_90     = 10,
-	ROTATION_ROLL_180_YAW_135    = 11,
-	ROTATION_PITCH_180           = 12,
-	ROTATION_ROLL_180_YAW_225    = 13,
-	ROTATION_ROLL_180_YAW_270    = 14,
-	ROTATION_ROLL_180_YAW_315    = 15,
-	ROTATION_ROLL_90             = 16,
-	ROTATION_ROLL_90_YAW_45      = 17,
-	ROTATION_ROLL_90_YAW_90      = 18,
-	ROTATION_ROLL_90_YAW_135     = 19,
-	ROTATION_ROLL_270            = 20,
-	ROTATION_ROLL_270_YAW_45     = 21,
-	ROTATION_ROLL_270_YAW_90     = 22,
-	ROTATION_ROLL_270_YAW_135    = 23,
-	ROTATION_PITCH_90            = 24,
-	ROTATION_PITCH_270           = 25,
-	ROTATION_MAX
-};
-
-typedef struct {
-	uint16_t roll;
-	uint16_t pitch;
-	uint16_t yaw;
-} rot_lookup_t;
-
-const rot_lookup_t rot_lookup[] = {
-	{  0,   0,   0 },
-	{  0,   0,  45 },
-	{  0,   0,  90 },
-	{  0,   0, 135 },
-	{  0,   0, 180 },
-	{  0,   0, 225 },
-	{  0,   0, 270 },
-	{  0,   0, 315 },
-	{180,   0,   0 },
-	{180,   0,  45 },
-	{180,   0,  90 },
-	{180,   0, 135 },
-	{  0, 180,   0 },
-	{180,   0, 225 },
-	{180,   0, 270 },
-	{180,   0, 315 },
-	{ 90,   0,   0 },
-	{ 90,   0,  45 },
-	{ 90,   0,  90 },
-	{ 90,   0, 135 },
-	{270,   0,   0 },
-	{270,   0,  45 },
-	{270,   0,  90 },
-	{270,   0, 135 },
-	{  0,  90,   0 },
-	{  0, 270,   0 }
-};
-
 /**
  * Sensor app start / stop handling function
  *
@@ -384,11 +316,6 @@ private:
 	 */
 	int		parameters_update();
 
-	/**
-	 * Get the rotation matrices
-	 */
-	void		get_rot_matrix(enum Rotation rot, math::Matrix *rot_matrix);
-
 	/**
 	 * Do accel-related initialisation.
 	 */
@@ -803,24 +730,6 @@ Sensors::parameters_update()
 	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()
 {

From b75c8e672fb401d9b1673d53a1972b9dddfa6b15 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sun, 20 Oct 2013 23:16:23 +0200
Subject: [PATCH 19/47] accelerometer calibration fix

---
 .../commander/accelerometer_calibration.cpp   | 45 +++++++------------
 1 file changed, 17 insertions(+), 28 deletions(-)

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index c9bfedbda0..b6aa64aa4d 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -129,7 +129,7 @@
 #endif
 static const int ERROR = -1;
 
-int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]);
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]);
 int detect_orientation(int mavlink_fd, int sub_sensor_combined);
 int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
 int mat_invert3(float src[3][3], float dst[3][3]);
@@ -142,8 +142,8 @@ int do_accel_calibration(int mavlink_fd) {
 
 	/* measure and calculate offsets & scales */
 	float accel_offs[3];
-	float accel_scale[3];
-	int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_scale);
+	float accel_T[3][3];
+	int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
 
 	if (res == OK) {
 		/* measurements complete successfully, rotate calibration values */
@@ -153,34 +153,31 @@ int do_accel_calibration(int mavlink_fd) {
 		math::Matrix board_rotation(3, 3);
 		get_rot_matrix(board_rotation_id, &board_rotation);
 		board_rotation = board_rotation.transpose();
-		math::Vector3 vect(3);
-		vect(0) = accel_offs[0];
-		vect(1) = accel_offs[1];
-		vect(2) = accel_offs[2];
-		math::Vector3 accel_offs_rotated = board_rotation * vect;
-		vect(0) = accel_scale[0];
-		vect(1) = accel_scale[1];
-		vect(2) = accel_scale[2];
-		math::Vector3 accel_scale_rotated = board_rotation * vect;
+		math::Vector3 accel_offs_vec;
+		accel_offs_vec.set(&accel_offs[0]);
+		math::Vector3 accel_offs_rotated = board_rotation * accel_offs_vec;
+		math::Matrix accel_T_mat(3, 3);
+		accel_T_mat.set(&accel_T[0][0]);
+		math::Matrix accel_T_rotated = board_rotation.transpose() * accel_T_mat * board_rotation;
 
 		/* set parameters */
 		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0)))
 			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1)))
 			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2)))
-			|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale_rotated(0)))
-			|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale_rotated(1)))
-			|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale_rotated(2)))) {
+			|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0)))
+			|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1)))
+			|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) {
 			mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
 		}
 
 		int fd = open(ACCEL_DEVICE_PATH, 0);
 		struct accel_scale ascale = {
 			accel_offs_rotated(0),
-			accel_scale_rotated(0),
+			accel_T_rotated(0, 0),
 			accel_offs_rotated(1),
-			accel_scale_rotated(1),
+			accel_T_rotated(1, 1),
 			accel_offs_rotated(2),
-			accel_scale_rotated(2),
+			accel_T_rotated(2, 2),
 		};
 
 		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
@@ -206,7 +203,7 @@ int do_accel_calibration(int mavlink_fd) {
 	/* exit accel calibration mode */
 }
 
-int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]) {
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) {
 	const int samples_num = 2500;
 	float accel_ref[6][3];
 	bool data_collected[6] = { false, false, false, false, false, false };
@@ -282,21 +279,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 	}
 	close(sensor_combined_sub);
 
-	/* calculate offsets and rotation+scale matrix */
-	float accel_T[3][3];
+	/* calculate offsets and transform matrix */
 	int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
 	if (res != 0) {
 		mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
 		return ERROR;
 	}
 
-	/* convert accel transform matrix to scales,
-	 * rotation part of transform matrix is not used by now
-	 */
-	for (int i = 0; i < 3; i++) {
-		accel_scale[i] = accel_T[i][i];
-	}
-
 	return OK;
 }
 

From 0dc9c9ac262c10866cbaf23ca98c8ce4582b5993 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sun, 20 Oct 2013 23:28:09 +0200
Subject: [PATCH 20/47] accelerometer_calibration: code style fixed,
 lib/conversion copyright fix

---
 src/lib/conversion/module.mk                  |   2 +-
 src/lib/conversion/rotation.cpp               |  40 +++++-
 src/lib/conversion/rotation.h                 |  40 +++++-
 .../commander/accelerometer_calibration.cpp   | 122 +++++++++++-------
 4 files changed, 149 insertions(+), 55 deletions(-)

diff --git a/src/lib/conversion/module.mk b/src/lib/conversion/module.mk
index 102711aaf7..f5f59a2dca 100644
--- a/src/lib/conversion/module.mk
+++ b/src/lib/conversion/module.mk
@@ -32,7 +32,7 @@
 ############################################################################
 
 #
-# Rotation library
+# Conversion library
 #
 
 SRCS		 = rotation.cpp
diff --git a/src/lib/conversion/rotation.cpp b/src/lib/conversion/rotation.cpp
index b65226cf1c..b078562c2d 100644
--- a/src/lib/conversion/rotation.cpp
+++ b/src/lib/conversion/rotation.cpp
@@ -1,8 +1,40 @@
-/*
- * rotation.cpp
+/****************************************************************************
  *
- *  Created on: 20.10.2013
- *      Author: ton
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rotation.cpp
+ *
+ * Vector rotation library
  */
 
 #include "math.h"
diff --git a/src/lib/conversion/rotation.h b/src/lib/conversion/rotation.h
index 97c6a09076..85c63c0fcb 100644
--- a/src/lib/conversion/rotation.h
+++ b/src/lib/conversion/rotation.h
@@ -1,8 +1,40 @@
-/*
- * rotation.h
+/****************************************************************************
  *
- *  Created on: 20.10.2013
- *      Author: ton
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file rotation.h
+ *
+ * Vector rotation library
  */
 
 #ifndef ROTATION_H_
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index b6aa64aa4d..4880af9071 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -135,7 +135,8 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 int mat_invert3(float src[3][3], float dst[3][3]);
 int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g);
 
-int do_accel_calibration(int mavlink_fd) {
+int do_accel_calibration(int mavlink_fd)
+{
 	/* announce change */
 	mavlink_log_info(mavlink_fd, "accel calibration started");
 	mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
@@ -162,11 +163,11 @@ int do_accel_calibration(int mavlink_fd) {
 
 		/* set parameters */
 		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0)))
-			|| param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1)))
-			|| param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2)))
-			|| param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0)))
-			|| param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1)))
-			|| param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) {
+		    || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1)))
+		    || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2)))
+		    || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0)))
+		    || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1)))
+		    || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) {
 			mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
 		}
 
@@ -194,6 +195,7 @@ int do_accel_calibration(int mavlink_fd) {
 
 		mavlink_log_info(mavlink_fd, "accel calibration done");
 		return OK;
+
 	} else {
 		/* measurements error */
 		mavlink_log_info(mavlink_fd, "accel calibration aborted");
@@ -203,7 +205,8 @@ int do_accel_calibration(int mavlink_fd) {
 	/* exit accel calibration mode */
 }
 
-int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]) {
+int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3])
+{
 	const int samples_num = 2500;
 	float accel_ref[6][3];
 	bool data_collected[6] = { false, false, false, false, false, false };
@@ -243,12 +246,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 		}
 
 		mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
-			(!data_collected[0]) ? "x+ " : "",
-			(!data_collected[1]) ? "x- " : "",
-			(!data_collected[2]) ? "y+ " : "",
-			(!data_collected[3]) ? "y- " : "",
-			(!data_collected[4]) ? "z+ " : "",
-			(!data_collected[5]) ? "z- " : "");
+				 (!data_collected[0]) ? "x+ " : "",
+				 (!data_collected[1]) ? "x- " : "",
+				 (!data_collected[2]) ? "y+ " : "",
+				 (!data_collected[3]) ? "y- " : "",
+				 (!data_collected[4]) ? "z+ " : "",
+				 (!data_collected[5]) ? "z- " : "");
 
 		if (old_done_count != done_count)
 			mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count);
@@ -257,6 +260,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 			break;
 
 		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
+
 		if (orient < 0) {
 			close(sensor_combined_sub);
 			return ERROR;
@@ -270,17 +274,19 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 		mavlink_log_info(mavlink_fd, "accel measurement started: %s axis", orientation_strs[orient]);
 		read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num);
 		mavlink_log_info(mavlink_fd, "result for %s axis: [ %.2f %.2f %.2f ]", orientation_strs[orient],
-			(double)accel_ref[orient][0],
-			(double)accel_ref[orient][1],
-			(double)accel_ref[orient][2]);
+				 (double)accel_ref[orient][0],
+				 (double)accel_ref[orient][1],
+				 (double)accel_ref[orient][2]);
 
 		data_collected[orient] = true;
 		tune_neutral();
 	}
+
 	close(sensor_combined_sub);
 
 	/* calculate offsets and transform matrix */
 	int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
+
 	if (res != 0) {
 		mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
 		return ERROR;
@@ -295,7 +301,8 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
  * @return 0..5 according to orientation when vehicle is still and ready for measurements,
  * ERROR if vehicle is not still after 30s or orientation error is more than 5m/s^2
  */
-int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
+int detect_orientation(int mavlink_fd, int sub_sensor_combined)
+{
 	struct sensor_combined_s sensor;
 	/* exponential moving average of accel */
 	float accel_ema[3] = { 0.0f, 0.0f, 0.0f };
@@ -326,30 +333,35 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 	while (true) {
 		/* wait blocking for new data */
 		int poll_ret = poll(fds, 1, 1000);
+
 		if (poll_ret) {
 			orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &sensor);
 			t = hrt_absolute_time();
 			float dt = (t - t_prev) / 1000000.0f;
 			t_prev = t;
 			float w = dt / ema_len;
+
 			for (int i = 0; i < 3; i++) {
 				accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w;
 				float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i];
 				d = d * d;
 				accel_disp[i] = accel_disp[i] * (1.0f - w);
+
 				if (d > accel_disp[i])
 					accel_disp[i] = d;
 			}
+
 			/* still detector with hysteresis */
-			if (  accel_disp[0] < still_thr2 &&
-				  accel_disp[1] < still_thr2 &&
-				  accel_disp[2] < still_thr2 ) {
+			if (accel_disp[0] < still_thr2 &&
+			    accel_disp[1] < still_thr2 &&
+			    accel_disp[2] < still_thr2) {
 				/* is still now */
 				if (t_still == 0) {
 					/* first time */
 					mavlink_log_info(mavlink_fd, "detected rest position, waiting...");
 					t_still = t;
 					t_timeout = t + timeout;
+
 				} else {
 					/* still since t_still */
 					if (t > t_still + still_time) {
@@ -357,18 +369,21 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 						break;
 					}
 				}
-			} else if ( accel_disp[0] > still_thr2 * 2.0f ||
-					    accel_disp[1] > still_thr2 * 2.0f ||
-					    accel_disp[2] > still_thr2 * 2.0f) {
+
+			} else if (accel_disp[0] > still_thr2 * 2.0f ||
+				   accel_disp[1] > still_thr2 * 2.0f ||
+				   accel_disp[2] > still_thr2 * 2.0f) {
 				/* not still, reset still start time */
 				if (t_still != 0) {
 					mavlink_log_info(mavlink_fd, "detected motion, please hold still...");
 					t_still = 0;
 				}
 			}
+
 		} else if (poll_ret == 0) {
 			poll_errcount++;
 		}
+
 		if (t > t_timeout) {
 			poll_errcount++;
 		}
@@ -379,29 +394,34 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 		}
 	}
 
-	if (  fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabsf(accel_ema[1]) < accel_err_thr &&
-		  fabsf(accel_ema[2]) < accel_err_thr  )
+	if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr)
 		return 0;	// [ g, 0, 0 ]
-	if (  fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabsf(accel_ema[1]) < accel_err_thr &&
-		  fabsf(accel_ema[2]) < accel_err_thr  )
+
+	if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr)
 		return 1;	// [ -g, 0, 0 ]
-	if (  fabsf(accel_ema[0]) < accel_err_thr &&
-		  fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabsf(accel_ema[2]) < accel_err_thr  )
+
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr)
 		return 2;	// [ 0, g, 0 ]
-	if (  fabsf(accel_ema[0]) < accel_err_thr &&
-		  fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
-		  fabsf(accel_ema[2]) < accel_err_thr  )
+
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
+	    fabsf(accel_ema[2]) < accel_err_thr)
 		return 3;	// [ 0, -g, 0 ]
-	if (  fabsf(accel_ema[0]) < accel_err_thr &&
-		  fabsf(accel_ema[1]) < accel_err_thr &&
-		  fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr  )
+
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr)
 		return 4;	// [ 0, 0, g ]
-	if (  fabsf(accel_ema[0]) < accel_err_thr &&
-		  fabsf(accel_ema[1]) < accel_err_thr &&
-		  fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr  )
+
+	if (fabsf(accel_ema[0]) < accel_err_thr &&
+	    fabsf(accel_ema[1]) < accel_err_thr &&
+	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
 		return 5;	// [ 0, 0, -g ]
 
 	mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
@@ -412,7 +432,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) {
 /*
  * Read specified number of accelerometer samples, calculate average and dispersion.
  */
-int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) {
+int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num)
+{
 	struct pollfd fds[1];
 	fds[0].fd = sensor_combined_sub;
 	fds[0].events = POLLIN;
@@ -423,12 +444,16 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 
 	while (count < samples_num) {
 		int poll_ret = poll(fds, 1, 1000);
+
 		if (poll_ret == 1) {
 			struct sensor_combined_s sensor;
 			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
+
 			for (int i = 0; i < 3; i++)
 				accel_sum[i] += sensor.accelerometer_m_s2[i];
+
 			count++;
+
 		} else {
 			errcount++;
 			continue;
@@ -445,10 +470,12 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 	return OK;
 }
 
-int mat_invert3(float src[3][3], float dst[3][3]) {
+int mat_invert3(float src[3][3], float dst[3][3])
+{
 	float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) -
-			    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
-			    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
+		    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
+		    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);
+
 	if (det == 0.0f)
 		return ERROR;	// Singular matrix
 
@@ -465,7 +492,8 @@ int mat_invert3(float src[3][3], float dst[3][3]) {
 	return OK;
 }
 
-int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g) {
+int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], float accel_offs[3], float g)
+{
 	/* calculate offsets */
 	for (int i = 0; i < 3; i++) {
 		accel_offs[i] = (accel_ref[i * 2][i] + accel_ref[i * 2 + 1][i]) / 2;
@@ -474,6 +502,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 	/* fill matrix A for linear equations system*/
 	float mat_A[3][3];
 	memset(mat_A, 0, sizeof(mat_A));
+
 	for (int i = 0; i < 3; i++) {
 		for (int j = 0; j < 3; j++) {
 			float a = accel_ref[i * 2][j] - accel_offs[j];
@@ -483,6 +512,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 
 	/* calculate inverse matrix for A */
 	float mat_A_inv[3][3];
+
 	if (mat_invert3(mat_A, mat_A_inv) != OK)
 		return ERROR;
 

From ed79b686c57f41d9d6bd3726fe0071e11740b3d7 Mon Sep 17 00:00:00 2001
From: Stefan Rado <px4@sradonia.net>
Date: Sun, 20 Oct 2013 00:08:36 +0200
Subject: [PATCH 21/47] Adjusted mavlink info messages during gyro calibration
 to not break QGroundControl.

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

diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index b6de5141f6..e1d6e8340d 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -129,7 +129,7 @@ int do_gyro_calibration(int mavlink_fd)
 	}
 
 	/* beep on calibration end */
-	mavlink_log_info(mavlink_fd, "offset calibration done.");
+	mavlink_log_info(mavlink_fd, "gyro offset calibration done.");
 	tune_neutral();
 
 	/* set offset parameters to new values */
@@ -240,7 +240,7 @@ int do_gyro_calibration(int mavlink_fd)
 	}
 
 	/* beep on calibration end */
-	mavlink_log_info(mavlink_fd, "scale calibration done.");
+	mavlink_log_info(mavlink_fd, "gyro scale calibration done.");
 	tune_neutral();
 
 #endif
@@ -270,6 +270,8 @@ int do_gyro_calibration(int mavlink_fd)
 		return ERROR;
 	}
 
+	mavlink_log_info(mavlink_fd, "gyro calibration done.");
+
 	close(sub_sensor_gyro);
 	return OK;
 }

From ea89f23c917733f8a682c82e24e1e4f223f79843 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Mon, 21 Oct 2013 20:07:47 +0200
Subject: [PATCH 22/47] calibration: bugs fixed, mavlink messages cleanup

---
 src/drivers/drv_accel.h                       |   2 +-
 .../commander/accelerometer_calibration.cpp   | 201 +++++++++-------
 src/modules/commander/commander.cpp           |  54 +++--
 src/modules/commander/gyro_calibration.cpp    | 219 ++++++++++--------
 4 files changed, 266 insertions(+), 210 deletions(-)

diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index eff5e73495..8a4f684284 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -66,7 +66,7 @@ struct accel_report {
 	int16_t temperature_raw;
 };
 
-/** accel scaling factors; Vout = (Vin * Vscale) + Voffset */
+/** accel scaling factors; Vout = Vscale * (Vin + Voffset) */
 struct accel_scale {
 	float	x_offset;
 	float	x_scale;
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 4880af9071..d11d7eb5d3 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -100,6 +100,24 @@
  * accel_T = A^-1 * g
  * g = 9.80665
  *
+ * ===== Rotation =====
+ *
+ * Calibrating using model:
+ * accel_corr = accel_T_r * (rot * accel_raw - accel_offs_r)
+ *
+ * Actual correction:
+ * accel_corr = rot * accel_T * (accel_raw - accel_offs)
+ *
+ * Known: accel_T_r, accel_offs_r, rot
+ * Unknown: accel_T, accel_offs
+ *
+ * Solution:
+ * accel_T_r * (rot * accel_raw - accel_offs_r) = rot * accel_T * (accel_raw - accel_offs)
+ * rot^-1 * accel_T_r * (rot * accel_raw - accel_offs_r) = accel_T * (accel_raw - accel_offs)
+ * rot^-1 * accel_T_r * rot * accel_raw - rot^-1 * accel_T_r * accel_offs_r = accel_T * accel_raw - accel_T * accel_offs)
+ * => accel_T = rot^-1 * accel_T_r * rot
+ * => accel_offs = rot^-1 * accel_offs_r
+ *
  * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
@@ -137,72 +155,97 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 
 int do_accel_calibration(int mavlink_fd)
 {
-	/* announce change */
-	mavlink_log_info(mavlink_fd, "accel calibration started");
-	mavlink_log_info(mavlink_fd, "accel cal progress <0> percent");
+	mavlink_log_info(mavlink_fd, "accel calibration: started");
+	mavlink_log_info(mavlink_fd, "accel calibration: progress <0>");
+
+	struct accel_scale accel_scale = {
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+		0.0f,
+		1.0f,
+	};
+
+	int res = OK;
+
+	/* reset all offsets to zero and all scales to one */
+	int fd = open(ACCEL_DEVICE_PATH, 0);
+	res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
+	close(fd);
+
+	if (res != OK) {
+		mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets");
+	}
 
 	/* measure and calculate offsets & scales */
 	float accel_offs[3];
 	float accel_T[3][3];
-	int res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
+	res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
 
 	if (res == OK) {
-		/* measurements complete successfully, rotate calibration values */
+		/* measurements completed successfully, rotate calibration values */
 		param_t board_rotation_h = param_find("SENS_BOARD_ROT");
-		enum Rotation board_rotation_id;
-		param_get(board_rotation_h, &(board_rotation_id));
+		int32_t board_rotation_int;
+		param_get(board_rotation_h, &(board_rotation_int));
+		enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
 		math::Matrix board_rotation(3, 3);
 		get_rot_matrix(board_rotation_id, &board_rotation);
-		board_rotation = board_rotation.transpose();
+		math::Matrix board_rotation_t = board_rotation.transpose();
 		math::Vector3 accel_offs_vec;
 		accel_offs_vec.set(&accel_offs[0]);
-		math::Vector3 accel_offs_rotated = board_rotation * accel_offs_vec;
+		math::Vector3 accel_offs_rotated = board_rotation_t * accel_offs_vec;
 		math::Matrix accel_T_mat(3, 3);
 		accel_T_mat.set(&accel_T[0][0]);
-		math::Matrix accel_T_rotated = board_rotation.transpose() * accel_T_mat * board_rotation;
+		math::Matrix accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
+
+		accel_scale.x_offset = accel_offs_rotated(0);
+		accel_scale.x_scale = accel_T_rotated(0, 0);
+		accel_scale.y_offset = accel_offs_rotated(1);
+		accel_scale.y_scale = accel_T_rotated(1, 1);
+		accel_scale.z_offset = accel_offs_rotated(2);
+		accel_scale.z_scale = accel_T_rotated(2, 2);
 
 		/* set parameters */
-		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_offs_rotated(0)))
-		    || param_set(param_find("SENS_ACC_YOFF"), &(accel_offs_rotated(1)))
-		    || param_set(param_find("SENS_ACC_ZOFF"), &(accel_offs_rotated(2)))
-		    || param_set(param_find("SENS_ACC_XSCALE"), &(accel_T_rotated(0, 0)))
-		    || param_set(param_find("SENS_ACC_YSCALE"), &(accel_T_rotated(1, 1)))
-		    || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_T_rotated(2, 2)))) {
-			mavlink_log_critical(mavlink_fd, "ERROR: setting offs or scale failed");
+		if (param_set(param_find("SENS_ACC_XOFF"), &(accel_scale.x_offset))
+		    || param_set(param_find("SENS_ACC_YOFF"), &(accel_scale.y_offset))
+		    || param_set(param_find("SENS_ACC_ZOFF"), &(accel_scale.z_offset))
+		    || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
+		    || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
+		    || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
+			mavlink_log_critical(mavlink_fd, "ERROR: setting accel params failed");
+			res = ERROR;
 		}
-
-		int fd = open(ACCEL_DEVICE_PATH, 0);
-		struct accel_scale ascale = {
-			accel_offs_rotated(0),
-			accel_T_rotated(0, 0),
-			accel_offs_rotated(1),
-			accel_T_rotated(1, 1),
-			accel_offs_rotated(2),
-			accel_T_rotated(2, 2),
-		};
-
-		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
-			warn("WARNING: failed to set scale / offsets for accel");
-
-		close(fd);
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
-
-		mavlink_log_info(mavlink_fd, "accel calibration done");
-		return OK;
-
-	} else {
-		/* measurements error */
-		mavlink_log_info(mavlink_fd, "accel calibration aborted");
-		return ERROR;
 	}
 
-	/* exit accel calibration mode */
+	if (res == OK) {
+		/* apply new scaling and offsets */
+		int fd = open(ACCEL_DEVICE_PATH, 0);
+		res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale);
+		close(fd);
+
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for accel");
+		}
+	}
+
+	if (res == OK) {
+		/* auto-save to EEPROM */
+		res = param_save_default();
+
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters");
+		}
+	}
+
+	if (res == OK) {
+		mavlink_log_info(mavlink_fd, "accel calibration: done");
+
+	} else {
+		mavlink_log_info(mavlink_fd, "accel calibration: failed");
+	}
+
+	return res;
 }
 
 int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3])
@@ -212,27 +255,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 	bool data_collected[6] = { false, false, false, false, false, false };
 	const char *orientation_strs[6] = { "x+", "x-", "y+", "y-", "z+", "z-" };
 
-	/* reset existing calibration */
-	int fd = open(ACCEL_DEVICE_PATH, 0);
-	struct accel_scale ascale_null = {
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-		0.0f,
-		1.0f,
-	};
-	int ioctl_res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale_null);
-	close(fd);
-
-	if (OK != ioctl_res) {
-		warn("ERROR: failed to set scale / offsets for accel");
-		return ERROR;
-	}
-
 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 
 	unsigned done_count = 0;
+	int res = OK;
 
 	while (true) {
 		bool done = true;
@@ -245,6 +271,12 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 			}
 		}
 
+		if (old_done_count != done_count)
+			mavlink_log_info(mavlink_fd, "accel calibration: progress <%u>", 17 * done_count);
+
+		if (done)
+			break;
+
 		mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
 				 (!data_collected[0]) ? "x+ " : "",
 				 (!data_collected[1]) ? "x- " : "",
@@ -253,17 +285,11 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 				 (!data_collected[4]) ? "z+ " : "",
 				 (!data_collected[5]) ? "z- " : "");
 
-		if (old_done_count != done_count)
-			mavlink_log_info(mavlink_fd, "accel cal progress <%u> percent", 17 * done_count);
-
-		if (done)
-			break;
-
 		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
 
 		if (orient < 0) {
-			close(sensor_combined_sub);
-			return ERROR;
+			res = ERROR;
+			break;
 		}
 
 		if (data_collected[orient]) {
@@ -284,15 +310,16 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 
 	close(sensor_combined_sub);
 
-	/* calculate offsets and transform matrix */
-	int res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
+	if (res == OK) {
+		/* calculate offsets and transform matrix */
+		res = calculate_calibration_values(accel_ref, accel_T, accel_offs, CONSTANTS_ONE_G);
 
-	if (res != 0) {
-		mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
-		return ERROR;
+		if (res != OK) {
+			mavlink_log_info(mavlink_fd, "ERROR: calibration values calculation error");
+		}
 	}
 
-	return OK;
+	return res;
 }
 
 /*
@@ -309,7 +336,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 	/* max-hold dispersion of accel */
 	float accel_disp[3] = { 0.0f, 0.0f, 0.0f };
 	/* EMA time constant in seconds*/
-	float ema_len = 0.2f;
+	float ema_len = 0.5f;
 	/* set "still" threshold to 0.25 m/s^2 */
 	float still_thr2 = pow(0.25f, 2);
 	/* set accel error threshold to 5m/s^2 */
@@ -342,8 +369,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 			float w = dt / ema_len;
 
 			for (int i = 0; i < 3; i++) {
-				accel_ema[i] = accel_ema[i] * (1.0f - w) + sensor.accelerometer_m_s2[i] * w;
-				float d = (float) sensor.accelerometer_m_s2[i] - accel_ema[i];
+				float d = sensor.accelerometer_m_s2[i] - accel_ema[i];
+				accel_ema[i] += d * w;
 				d = d * d;
 				accel_disp[i] = accel_disp[i] * (1.0f - w);
 
@@ -389,8 +416,8 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 		}
 
 		if (poll_errcount > 1000) {
-			mavlink_log_info(mavlink_fd, "ERROR: Failed reading sensor");
-			return -1;
+			mavlink_log_critical(mavlink_fd, "ERROR: failed reading sensor");
+			return ERROR;
 		}
 	}
 
@@ -424,9 +451,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
 		return 5;	// [ 0, 0, -g ]
 
-	mavlink_log_info(mavlink_fd, "ERROR: invalid orientation");
+	mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");
 
-	return -2;	// Can't detect orientation
+	return ERROR;	// Can't detect orientation
 }
 
 /*
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 2ef509980c..9545ef171d 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -369,8 +369,10 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			if (hil_ret == OK && control_mode->flag_system_hil_enabled) {
 				/* reset the arming mode to disarmed */
 				arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_STANDBY, armed);
+
 				if (arming_res != TRANSITION_DENIED) {
 					mavlink_log_info(mavlink_fd, "[cmd] HIL: Reset ARMED state to standby");
+
 				} else {
 					mavlink_log_info(mavlink_fd, "[cmd] HIL: FAILED resetting armed state");
 				}
@@ -481,27 +483,28 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			break;
 		}
 
-	case VEHICLE_CMD_COMPONENT_ARM_DISARM:
-	{
-		transition_result_t arming_res = TRANSITION_NOT_CHANGED;
-		if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
-			if (safety->safety_switch_available && !safety->safety_off) {
-				print_reject_arm("NOT ARMING: Press safety switch first.");
-				arming_res = TRANSITION_DENIED;
+	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
+			transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
-			} else {
-				arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
-			}
+			if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) {
+				if (safety->safety_switch_available && !safety->safety_off) {
+					print_reject_arm("NOT ARMING: Press safety switch first.");
+					arming_res = TRANSITION_DENIED;
 
-			if (arming_res == TRANSITION_CHANGED) {
-				mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
-				result = VEHICLE_CMD_RESULT_ACCEPTED;
-			} else {
-				mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
-				result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				} else {
+					arming_res = arming_state_transition(status, safety, control_mode, ARMING_STATE_ARMED, armed);
+				}
+
+				if (arming_res == TRANSITION_CHANGED) {
+					mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+
+				} else {
+					mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				}
 			}
 		}
-	}
 		break;
 
 	default:
@@ -940,7 +943,7 @@ int commander_thread_main(int argc, char *argv[])
 			last_idle_time = system_load.tasks[0].total_runtime;
 
 			/* check if board is connected via USB */
-			struct stat statbuf;
+			//struct stat statbuf;
 			//on_usb_power = (stat("/dev/ttyACM0", &statbuf) == 0);
 		}
 
@@ -970,6 +973,7 @@ int commander_thread_main(int argc, char *argv[])
 
 				if (armed.armed) {
 					arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_ARMED_ERROR, &armed);
+
 				} else {
 					arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY_ERROR, &armed);
 				}
@@ -1244,12 +1248,14 @@ int commander_thread_main(int argc, char *argv[])
 		counter++;
 
 		int blink_state = blink_msg_state();
+
 		if (blink_state > 0) {
 			/* blinking LED message, don't touch LEDs */
 			if (blink_state == 2) {
 				/* blinking LED message completed, restore normal state */
 				control_status_leds(&status, &armed, true);
 			}
+
 		} else {
 			/* normal state */
 			control_status_leds(&status, &armed, status_changed);
@@ -1264,7 +1270,7 @@ int commander_thread_main(int argc, char *argv[])
 	ret = pthread_join(commander_low_prio_thread, NULL);
 
 	if (ret) {
-		warn("join failed", ret);
+		warn("join failed: %d", ret);
 	}
 
 	rgbled_set_mode(RGBLED_MODE_OFF);
@@ -1308,6 +1314,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
 	/* driving rgbled */
 	if (changed) {
 		bool set_normal_color = false;
+
 		/* set mode */
 		if (status->arming_state == ARMING_STATE_ARMED) {
 			rgbled_set_mode(RGBLED_MODE_ON);
@@ -1332,6 +1339,7 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
 				if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
 					rgbled_set_color(RGBLED_COLOR_AMBER);
 				}
+
 				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
 
 			} else {
@@ -1694,11 +1702,10 @@ void *commander_low_prio_loop(void *arg)
 	fds[0].events = POLLIN;
 
 	while (!thread_should_exit) {
-
-		/* wait for up to 100ms for data */
+		/* wait for up to 200ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);
 
-		/* timed out - periodic check for _task_should_exit, etc. */
+		/* timed out - periodic check for thread_should_exit, etc. */
 		if (pret == 0)
 			continue;
 
@@ -1773,7 +1780,7 @@ void *commander_low_prio_loop(void *arg)
 
 				} else if ((int)(cmd.param4) == 1) {
 					/* RC calibration */
-					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 					calib_ret = do_rc_calibration(mavlink_fd);
 
 				} else if ((int)(cmd.param5) == 1) {
@@ -1854,7 +1861,6 @@ void *commander_low_prio_loop(void *arg)
 			/* send acknowledge command */
 			// XXX TODO
 		}
-
 	}
 
 	close(cmd_sub);
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index e1d6e8340d..219ae6cb2f 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -58,7 +58,7 @@ static const int ERROR = -1;
 
 int do_gyro_calibration(int mavlink_fd)
 {
-	mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
+	mavlink_log_info(mavlink_fd, "gyro calibration: started");
 
 	struct gyro_scale gyro_scale = {
 		0.0f,
@@ -69,79 +69,85 @@ int do_gyro_calibration(int mavlink_fd)
 		1.0f,
 	};
 
+	int res = OK;
+
+	/* reset all offsets to zero and all scales to one */
+	int fd = open(GYRO_DEVICE_PATH, 0);
+	res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
+	close(fd);
+
+	if (res != OK) {
+		mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets");
+	}
+
 	/* subscribe to gyro sensor topic */
 	int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
 	struct gyro_report gyro_report;
 
-	/* reset all offsets to zero and all scales to one */
-	int fd = open(GYRO_DEVICE_PATH, 0);
+	if (res == OK) {
+		/* determine gyro mean values */
+		const unsigned calibration_count = 5000;
+		unsigned calibration_counter = 0;
+		unsigned poll_errcount = 0;
 
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
-		warn("WARNING: failed to reset scale / offsets for gyro");
+		while (calibration_counter < calibration_count) {
+			/* wait blocking for new data */
+			struct pollfd fds[1];
+			fds[0].fd = sub_sensor_gyro;
+			fds[0].events = POLLIN;
 
-	close(fd);
+			int poll_ret = poll(fds, 1, 1000);
 
+			if (poll_ret > 0) {
+				orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
+				gyro_scale.x_offset += gyro_report.x;
+				gyro_scale.y_offset += gyro_report.y;
+				gyro_scale.z_offset += gyro_report.z;
+				calibration_counter++;
 
-	/*** --- OFFSETS --- ***/
+				if (calibration_counter % (calibration_count / 20) == 0)
+					mavlink_log_info(mavlink_fd, "gyro calibration: progress <%u>", (calibration_counter * 100) / calibration_count);
 
-	/* determine gyro mean values */
-	const unsigned calibration_count = 5000;
-	unsigned calibration_counter = 0;
-	unsigned poll_errcount = 0;
+			} else {
+				poll_errcount++;
+			}
 
-	while (calibration_counter < calibration_count) {
-
-		/* wait blocking for new data */
-		struct pollfd fds[1];
-		fds[0].fd = sub_sensor_gyro;
-		fds[0].events = POLLIN;
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret > 0) {
-			orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report);
-			gyro_scale.x_offset += gyro_report.x;
-			gyro_scale.y_offset += gyro_report.y;
-			gyro_scale.z_offset += gyro_report.z;
-			calibration_counter++;
-			if (calibration_counter % (calibration_count / 20) == 0)
-				mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count);
-
-		} else {
-			poll_errcount++;
+			if (poll_errcount > 1000) {
+				mavlink_log_critical(mavlink_fd, "ERROR: failed reading gyro sensor");
+				res = ERROR;
+				break;
+			}
 		}
 
-		if (poll_errcount > 1000) {
-			mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
-			close(sub_sensor_gyro);
-			return ERROR;
+		gyro_scale.x_offset /= calibration_count;
+		gyro_scale.y_offset /= calibration_count;
+		gyro_scale.z_offset /= calibration_count;
+	}
+
+	if (res == OK) {
+		/* check offsets */
+		if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
+			mavlink_log_critical(mavlink_fd, "ERROR: offset is NaN");
+			res = ERROR;
 		}
 	}
 
-	gyro_scale.x_offset /= calibration_count;
-	gyro_scale.y_offset /= calibration_count;
-	gyro_scale.z_offset /= calibration_count;
-
-	if (!isfinite(gyro_scale.x_offset) || !isfinite(gyro_scale.y_offset) || !isfinite(gyro_scale.z_offset)) {
-		mavlink_log_info(mavlink_fd, "gyro offset calibration FAILED (NaN)");
-		close(sub_sensor_gyro);
-		return ERROR;
+	if (res == OK) {
+		/* set offset parameters to new values */
+		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
+		    || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
+		    || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
+			mavlink_log_critical(mavlink_fd, "ERROR: setting gyro offs params failed");
+			res = ERROR;
+		}
 	}
 
-	/* beep on calibration end */
-	mavlink_log_info(mavlink_fd, "gyro offset calibration done.");
+#if 0
+	/* beep on offset calibration end */
+	mavlink_log_info(mavlink_fd, "gyro offset calibration done");
 	tune_neutral();
 
-	/* set offset parameters to new values */
-	if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
-		|| param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
-		|| param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
-		mavlink_log_critical(mavlink_fd, "Setting gyro offset parameters failed!");
-	}
-
-
-	/*** --- SCALING --- ***/
-#if 0
+	/* scale calibration */
 	/* this was only a proof of concept and is currently not working. scaling will be set to 1.0 for now. */
 
 	mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
@@ -163,9 +169,11 @@ int do_gyro_calibration(int mavlink_fd)
 	// XXX change to mag topic
 	orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw);
 
-	float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
-	if (mag_last > M_PI_F) mag_last -= 2*M_PI_F;
-	if (mag_last < -M_PI_F) mag_last += 2*M_PI_F;
+	float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
+
+	if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;
+
+	if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;
 
 
 	uint64_t last_time = hrt_absolute_time();
@@ -175,7 +183,7 @@ int do_gyro_calibration(int mavlink_fd)
 
 		/* abort this loop if not rotated more than 180 degrees within 5 seconds */
 		if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
-			&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
+		    && (hrt_absolute_time() - start_time > 5 * 1e6)) {
 			mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
 			close(sub_sensor_combined);
 			return OK;
@@ -203,14 +211,17 @@ int do_gyro_calibration(int mavlink_fd)
 			// calculate error between estimate and measurement
 			// apply declination correction for true heading as well.
 			//float mag = -atan2f(magNav(1),magNav(0));
-			float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]);
-			if (mag > M_PI_F) mag -= 2*M_PI_F;
-			if (mag < -M_PI_F) mag += 2*M_PI_F;
+			float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);
+
+			if (mag > M_PI_F) mag -= 2 * M_PI_F;
+
+			if (mag < -M_PI_F) mag += 2 * M_PI_F;
 
 			float diff = mag - mag_last;
 
-			if (diff > M_PI_F) diff -= 2*M_PI_F;
-			if (diff < -M_PI_F) diff += 2*M_PI_F;
+			if (diff > M_PI_F) diff -= 2 * M_PI_F;
+
+			if (diff < -M_PI_F) diff += 2 * M_PI_F;
 
 			baseline_integral += diff;
 			mag_last = mag;
@@ -220,15 +231,15 @@ int do_gyro_calibration(int mavlink_fd)
 
 //			warnx("dbg: b: %6.4f, g: %6.4f", (double)baseline_integral, (double)gyro_integral);
 
-		// } else if (poll_ret == 0) {
-		// 	/* any poll failure for 1s is a reason to abort */
-		// 	mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
-		// 	return;
+			// } else if (poll_ret == 0) {
+			// 	/* any poll failure for 1s is a reason to abort */
+			// 	mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry");
+			// 	return;
 		}
 	}
 
 	float gyro_scale = baseline_integral / gyro_integral;
-	
+
 	warnx("gyro scale: yaw (z): %6.4f", (double)gyro_scale);
 	mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", (double)gyro_scale);
 
@@ -236,42 +247,54 @@ int do_gyro_calibration(int mavlink_fd)
 	if (!isfinite(gyro_scale.x_scale) || !isfinite(gyro_scale.y_scale) || !isfinite(gyro_scale.z_scale)) {
 		mavlink_log_info(mavlink_fd, "gyro scale calibration FAILED (NaN)");
 		close(sub_sensor_gyro);
+		mavlink_log_critical(mavlink_fd, "gyro calibration failed");
 		return ERROR;
 	}
 
 	/* beep on calibration end */
-	mavlink_log_info(mavlink_fd, "gyro scale calibration done.");
+	mavlink_log_info(mavlink_fd, "gyro scale calibration done");
 	tune_neutral();
 
 #endif
 
-	/* set scale parameters to new values */
-	if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
-		|| param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
-		|| param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
-		mavlink_log_critical(mavlink_fd, "Setting gyro scale parameters failed!");
-	}
-
-	/* apply new scaling and offsets */
-	fd = open(GYRO_DEVICE_PATH, 0);
-
-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
-		warn("WARNING: failed to apply new scale for gyro");
-
-	close(fd);
-
-	/* auto-save to EEPROM */
-	int save_ret = param_save_default();
-
-	if (save_ret != 0) {
-		warnx("WARNING: auto-save of params to storage failed");
-		mavlink_log_critical(mavlink_fd, "gyro store failed");
-		close(sub_sensor_gyro);
-		return ERROR;
-	}
-
-	mavlink_log_info(mavlink_fd, "gyro calibration done.");
-
 	close(sub_sensor_gyro);
-	return OK;
+
+	if (res == OK) {
+		/* set scale parameters to new values */
+		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
+		    || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
+		    || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
+			mavlink_log_critical(mavlink_fd, "ERROR: setting gyro scale params failed");
+			res = ERROR;
+		}
+	}
+
+	if (res == OK) {
+		/* apply new scaling and offsets */
+		fd = open(GYRO_DEVICE_PATH, 0);
+		res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale);
+		close(fd);
+
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for gyro");
+		}
+	}
+
+	if (res == OK) {
+		/* auto-save to EEPROM */
+		res = param_save_default();
+
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters");
+		}
+	}
+
+	if (res == OK) {
+		mavlink_log_info(mavlink_fd, "gyro calibration: done");
+
+	} else {
+		mavlink_log_info(mavlink_fd, "gyro calibration: failed");
+	}
+
+	return res;
 }

From ef42ef15c6991800111b25374b0f6e3935c2a9a9 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Mon, 21 Oct 2013 22:24:59 +0200
Subject: [PATCH 23/47] accel/gyro/mag calibration: big cleanup, use common
 messages

---
 .../commander/accelerometer_calibration.cpp   |  40 ++-
 src/modules/commander/calibration_messages.h  |  57 ++++
 src/modules/commander/gyro_calibration.cpp    |  37 ++-
 src/modules/commander/mag_calibration.cpp     | 302 +++++++++---------
 4 files changed, 252 insertions(+), 184 deletions(-)
 create mode 100644 src/modules/commander/calibration_messages.h

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index d11d7eb5d3..49a8d6b339 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -122,6 +122,7 @@
  */
 
 #include "accelerometer_calibration.h"
+#include "calibration_messages.h"
 #include "commander_helper.h"
 
 #include <unistd.h>
@@ -147,6 +148,8 @@
 #endif
 static const int ERROR = -1;
 
+static const char *sensor_name = "accel";
+
 int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_T[3][3]);
 int detect_orientation(int mavlink_fd, int sub_sensor_combined);
 int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num);
@@ -155,8 +158,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 
 int do_accel_calibration(int mavlink_fd)
 {
-	mavlink_log_info(mavlink_fd, "accel calibration: started");
-	mavlink_log_info(mavlink_fd, "accel calibration: progress <0>");
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
 
 	struct accel_scale accel_scale = {
 		0.0f,
@@ -175,7 +177,7 @@ int do_accel_calibration(int mavlink_fd)
 	close(fd);
 
 	if (res != OK) {
-		mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets");
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
 	}
 
 	/* measure and calculate offsets & scales */
@@ -213,7 +215,7 @@ int do_accel_calibration(int mavlink_fd)
 		    || param_set(param_find("SENS_ACC_XSCALE"), &(accel_scale.x_scale))
 		    || param_set(param_find("SENS_ACC_YSCALE"), &(accel_scale.y_scale))
 		    || param_set(param_find("SENS_ACC_ZSCALE"), &(accel_scale.z_scale))) {
-			mavlink_log_critical(mavlink_fd, "ERROR: setting accel params failed");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			res = ERROR;
 		}
 	}
@@ -225,7 +227,7 @@ int do_accel_calibration(int mavlink_fd)
 		close(fd);
 
 		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for accel");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
 		}
 	}
 
@@ -234,15 +236,15 @@ int do_accel_calibration(int mavlink_fd)
 		res = param_save_default();
 
 		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
 		}
 	}
 
 	if (res == OK) {
-		mavlink_log_info(mavlink_fd, "accel calibration: done");
+		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
 
 	} else {
-		mavlink_log_info(mavlink_fd, "accel calibration: failed");
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 	}
 
 	return res;
@@ -266,13 +268,16 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 		done_count = 0;
 
 		for (int i = 0; i < 6; i++) {
-			if (!data_collected[i]) {
+			if (data_collected[i]) {
+				done_count++;
+
+			} else {
 				done = false;
 			}
 		}
 
 		if (old_done_count != done_count)
-			mavlink_log_info(mavlink_fd, "accel calibration: progress <%u>", 17 * done_count);
+			mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
 
 		if (done)
 			break;
@@ -293,7 +298,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 		}
 
 		if (data_collected[orient]) {
-			mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]);
+			mavlink_log_info(mavlink_fd, "%s done, rotate to a different axis", orientation_strs[orient]);
 			continue;
 		}
 
@@ -374,6 +379,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 				d = d * d;
 				accel_disp[i] = accel_disp[i] * (1.0f - w);
 
+				if (d > still_thr2 * 8.0f)
+					d = still_thr2 * 8.0f;
+
 				if (d > accel_disp[i])
 					accel_disp[i] = d;
 			}
@@ -397,12 +405,12 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 					}
 				}
 
-			} else if (accel_disp[0] > still_thr2 * 2.0f ||
-				   accel_disp[1] > still_thr2 * 2.0f ||
-				   accel_disp[2] > still_thr2 * 2.0f) {
+			} else if (accel_disp[0] > still_thr2 * 4.0f ||
+				   accel_disp[1] > still_thr2 * 4.0f ||
+				   accel_disp[2] > still_thr2 * 4.0f) {
 				/* not still, reset still start time */
 				if (t_still != 0) {
-					mavlink_log_info(mavlink_fd, "detected motion, please hold still...");
+					mavlink_log_info(mavlink_fd, "detected motion, hold still...");
 					t_still = 0;
 				}
 			}
@@ -416,7 +424,7 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 		}
 
 		if (poll_errcount > 1000) {
-			mavlink_log_critical(mavlink_fd, "ERROR: failed reading sensor");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
 			return ERROR;
 		}
 	}
diff --git a/src/modules/commander/calibration_messages.h b/src/modules/commander/calibration_messages.h
new file mode 100644
index 0000000000..fd8b8564df
--- /dev/null
+++ b/src/modules/commander/calibration_messages.h
@@ -0,0 +1,57 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: Anton Babushkin <anton.babushkin@me.com>
+ *
+ * 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 calibration_messages.h
+ *
+ * Common calibration messages.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#ifndef CALIBRATION_MESSAGES_H_
+#define CALIBRATION_MESSAGES_H_
+
+#define CAL_STARTED_MSG	"%s calibration: started"
+#define CAL_DONE_MSG	"%s calibration: done"
+#define CAL_FAILED_MSG	"%s calibration: failed"
+#define CAL_PROGRESS_MSG	"%s calibration: progress <%u>"
+
+#define CAL_FAILED_SENSOR_MSG	"ERROR: failed reading sensor"
+#define CAL_FAILED_RESET_CAL_MSG	"ERROR: failed to reset calibration"
+#define CAL_FAILED_APPLY_CAL_MSG	"ERROR: failed to apply calibration"
+#define CAL_FAILED_SET_PARAMS_MSG	"ERROR: failed to set parameters"
+#define CAL_FAILED_SAVE_PARAMS_MSG	"ERROR: failed to save parameters"
+
+#endif /* CALIBRATION_MESSAGES_H_ */
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 219ae6cb2f..30cd0d48d4 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -33,10 +33,12 @@
 
 /**
  * @file gyro_calibration.cpp
+ *
  * Gyroscope calibration routine
  */
 
 #include "gyro_calibration.h"
+#include "calibration_messages.h"
 #include "commander_helper.h"
 
 #include <stdio.h>
@@ -56,9 +58,12 @@
 #endif
 static const int ERROR = -1;
 
+static const char *sensor_name = "gyro";
+
 int do_gyro_calibration(int mavlink_fd)
 {
-	mavlink_log_info(mavlink_fd, "gyro calibration: started");
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, "don't move system");
 
 	struct gyro_scale gyro_scale = {
 		0.0f,
@@ -77,19 +82,19 @@ int do_gyro_calibration(int mavlink_fd)
 	close(fd);
 
 	if (res != OK) {
-		mavlink_log_critical(mavlink_fd, "ERROR: failed to reset scale / offsets");
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
 	}
 
-	/* subscribe to gyro sensor topic */
-	int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
-	struct gyro_report gyro_report;
-
 	if (res == OK) {
 		/* determine gyro mean values */
 		const unsigned calibration_count = 5000;
 		unsigned calibration_counter = 0;
 		unsigned poll_errcount = 0;
 
+		/* subscribe to gyro sensor topic */
+		int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+		struct gyro_report gyro_report;
+
 		while (calibration_counter < calibration_count) {
 			/* wait blocking for new data */
 			struct pollfd fds[1];
@@ -106,19 +111,21 @@ int do_gyro_calibration(int mavlink_fd)
 				calibration_counter++;
 
 				if (calibration_counter % (calibration_count / 20) == 0)
-					mavlink_log_info(mavlink_fd, "gyro calibration: progress <%u>", (calibration_counter * 100) / calibration_count);
+					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
 
 			} else {
 				poll_errcount++;
 			}
 
 			if (poll_errcount > 1000) {
-				mavlink_log_critical(mavlink_fd, "ERROR: failed reading gyro sensor");
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
 				res = ERROR;
 				break;
 			}
 		}
 
+		close(sub_sensor_gyro);
+
 		gyro_scale.x_offset /= calibration_count;
 		gyro_scale.y_offset /= calibration_count;
 		gyro_scale.z_offset /= calibration_count;
@@ -137,7 +144,7 @@ int do_gyro_calibration(int mavlink_fd)
 		if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_scale.x_offset))
 		    || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_scale.y_offset))
 		    || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_scale.z_offset))) {
-			mavlink_log_critical(mavlink_fd, "ERROR: setting gyro offs params failed");
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to set offset params");
 			res = ERROR;
 		}
 	}
@@ -257,14 +264,12 @@ int do_gyro_calibration(int mavlink_fd)
 
 #endif
 
-	close(sub_sensor_gyro);
-
 	if (res == OK) {
 		/* set scale parameters to new values */
 		if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scale.x_scale))
 		    || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scale.y_scale))
 		    || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scale.z_scale))) {
-			mavlink_log_critical(mavlink_fd, "ERROR: setting gyro scale params failed");
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params");
 			res = ERROR;
 		}
 	}
@@ -276,7 +281,7 @@ int do_gyro_calibration(int mavlink_fd)
 		close(fd);
 
 		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for gyro");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
 		}
 	}
 
@@ -285,15 +290,15 @@ int do_gyro_calibration(int mavlink_fd)
 		res = param_save_default();
 
 		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
 		}
 	}
 
 	if (res == OK) {
-		mavlink_log_info(mavlink_fd, "gyro calibration: done");
+		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
 
 	} else {
-		mavlink_log_info(mavlink_fd, "gyro calibration: failed");
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 	}
 
 	return res;
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index b0d4266be5..09f4104f89 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -33,12 +33,14 @@
 
 /**
  * @file mag_calibration.cpp
+ *
  * Magnetometer calibration routine
  */
 
 #include "mag_calibration.h"
 #include "commander_helper.h"
 #include "calibration_routines.h"
+#include "calibration_messages.h"
 
 #include <stdio.h>
 #include <stdlib.h>
@@ -59,26 +61,20 @@
 #endif
 static const int ERROR = -1;
 
+static const char *sensor_name = "mag";
+
 int do_mag_calibration(int mavlink_fd)
 {
-	mavlink_log_info(mavlink_fd, "please put the system in a rest position and wait.");
-
-	int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
-	struct mag_report mag;
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, "don't move system");
 
 	/* 45 seconds */
 	uint64_t calibration_interval = 45 * 1000 * 1000;
 
-	/* maximum 2000 values */
+	/* maximum 500 values */
 	const unsigned int calibration_maxcount = 500;
 	unsigned int calibration_counter = 0;
 
-	/* limit update rate to get equally spaced measurements over time (in ms) */
-	orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
-
-	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
-
-	/* erase old calibration */
 	struct mag_scale mscale_null = {
 		0.0f,
 		1.0f,
@@ -88,97 +84,92 @@ int do_mag_calibration(int mavlink_fd)
 		1.0f,
 	};
 
-	if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) {
-		warn("WARNING: failed to set scale / offsets for mag");
-		mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag");
+	int res = OK;
+
+	/* erase old calibration */
+	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
+
+	if (res != OK) {
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
 	}
 
-	/* calibrate range */
-	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
-		warnx("failed to calibrate scale");
+	if (res == OK) {
+		/* calibrate range */
+		res = ioctl(fd, MAGIOCCALIBRATE, fd);
+
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale");
+		}
 	}
 
 	close(fd);
 
-	mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");
+	float *x;
+	float *y;
+	float *z;
 
-	/* calibrate offsets */
+	if (res == OK) {
+		/* allocate memory */
+		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
 
-	// uint64_t calibration_start = hrt_absolute_time();
+		x = (float *)malloc(sizeof(float) * calibration_maxcount);
+		y = (float *)malloc(sizeof(float) * calibration_maxcount);
+		z = (float *)malloc(sizeof(float) * calibration_maxcount);
 
-	uint64_t axis_deadline = hrt_absolute_time();
-	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
-
-	const char axislabels[3] = { 'X', 'Y', 'Z'};
-	int axis_index = -1;
-
-	float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
-
-	if (x == NULL || y == NULL || z == NULL) {
-		warnx("mag cal failed: out of memory");
-		mavlink_log_info(mavlink_fd, "mag cal failed: out of memory");
-		warnx("x:%p y:%p z:%p\n", x, y, z);
-		return ERROR;
+		if (x == NULL || y == NULL || z == NULL) {
+			mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
+			res = ERROR;
+		}
 	}
 
-	mavlink_log_info(mavlink_fd, "scale calibration completed, dynamic calibration starting..");
+	if (res == OK) {
+		int sub_mag = orb_subscribe(ORB_ID(sensor_mag));
+		struct mag_report mag;
 
-	unsigned poll_errcount = 0;
+		/* limit update rate to get equally spaced measurements over time (in ms) */
+		orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount);
 
-	while (hrt_absolute_time() < calibration_deadline &&
-	       calibration_counter < calibration_maxcount) {
+		/* calibrate offsets */
+		uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
+		unsigned poll_errcount = 0;
 
-		/* wait blocking for new data */
-		struct pollfd fds[1];
-		fds[0].fd = sub_mag;
-		fds[0].events = POLLIN;
+		mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
 
-		/* user guidance */
-		if (hrt_absolute_time() >= axis_deadline &&
-		    axis_index < 3) {
+		while (hrt_absolute_time() < calibration_deadline &&
+		       calibration_counter < calibration_maxcount) {
 
-			axis_index++;
+			/* wait blocking for new data */
+			struct pollfd fds[1];
+			fds[0].fd = sub_mag;
+			fds[0].events = POLLIN;
 
-			mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]);
-			tune_neutral();
+			int poll_ret = poll(fds, 1, 1000);
 
-			axis_deadline += calibration_interval / 3;
+			if (poll_ret > 0) {
+				orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
+
+				x[calibration_counter] = mag.x;
+				y[calibration_counter] = mag.y;
+				z[calibration_counter] = mag.z;
+
+				calibration_counter++;
+
+				if (calibration_counter % (calibration_maxcount / 20) == 0)
+					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
+
+			} else {
+				poll_errcount++;
+			}
+
+			if (poll_errcount > 1000) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+				res = ERROR;
+				break;
+			}
 		}
 
-		if (!(axis_index < 3)) {
-			break;
-		}
-
-		int poll_ret = poll(fds, 1, 1000);
-
-		if (poll_ret > 0) {
-			orb_copy(ORB_ID(sensor_mag), sub_mag, &mag);
-
-			x[calibration_counter] = mag.x;
-			y[calibration_counter] = mag.y;
-			z[calibration_counter] = mag.z;
-
-			calibration_counter++;
-			if (calibration_counter % (calibration_maxcount / 20) == 0)
-				mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount);
-
-
-		} else {
-			poll_errcount++;
-		}
-
-		if (poll_errcount > 1000) {
-			mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
-			close(sub_mag);
-			free(x);
-			free(y);
-			free(z);
-			return ERROR;
-		}
-
-
+		close(sub_mag);
 	}
 
 	float sphere_x;
@@ -186,93 +177,100 @@ int do_mag_calibration(int mavlink_fd)
 	float sphere_z;
 	float sphere_radius;
 
-	mavlink_log_info(mavlink_fd, "mag cal progress <70> percent");
-	sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
-	mavlink_log_info(mavlink_fd, "mag cal progress <80> percent");
+	if (res == OK) {
+		/* sphere fit */
+		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
+		sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
+		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80);
 
-	free(x);
-	free(y);
-	free(z);
+		if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) {
+			mavlink_log_critical(mavlink_fd, "ERROR: NaN in sphere fit");
+			res = ERROR;
+		}
+	}
 
-	if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) {
+	if (x != NULL)
+		free(x);
 
-		fd = open(MAG_DEVICE_PATH, 0);
+	if (y != NULL)
+		free(y);
 
+	if (z != NULL)
+		free(z);
+
+	if (res == OK) {
+		/* apply calibration and set parameters */
 		struct mag_scale mscale;
 
-		if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to get scale / offsets for mag");
+		fd = open(MAG_DEVICE_PATH, 0);
+		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
 
-		mscale.x_offset = sphere_x;
-		mscale.y_offset = sphere_y;
-		mscale.z_offset = sphere_z;
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
+		}
 
-		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to set scale / offsets for mag");
+		if (res == OK) {
+			mscale.x_offset = sphere_x;
+			mscale.y_offset = sphere_y;
+			mscale.z_offset = sphere_z;
+
+			res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);
+
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+			}
+		}
 
 		close(fd);
 
-		/* announce and set new offset */
+		if (res == OK) {
+			/* set parameters */
+			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
+				res = ERROR;
 
-		if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
-			warnx("Setting X mag offset failed!\n");
+			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
+				res = ERROR;
+
+			if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
+				res = ERROR;
+
+			if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
+				res = ERROR;
+
+			if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
+				res = ERROR;
+
+			if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
+				res = ERROR;
+
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+			}
+
+			mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
 		}
 
-		if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
-			warnx("Setting Y mag offset failed!\n");
+		if (res == OK) {
+			/* auto-save to EEPROM */
+			res = param_save_default();
+
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+			}
 		}
 
-		if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
-			warnx("Setting Z mag offset failed!\n");
+		mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
+				 (double)mscale.y_offset, (double)mscale.z_offset);
+		mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
+				 (double)mscale.y_scale, (double)mscale.z_scale);
+
+		if (res == OK) {
+			mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+
+		} else {
+			mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 		}
 
-		if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
-			warnx("Setting X mag scale failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
-			warnx("Setting Y mag scale failed!\n");
-		}
-
-		if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
-			warnx("Setting Z mag scale failed!\n");
-		}
-
-		mavlink_log_info(mavlink_fd, "mag cal progress <90> percent");
-
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
-
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
-			close(sub_mag);
-			return ERROR;
-		}
-
-		warnx("\tscale: %.6f %.6f %.6f\n         \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n",
-		       (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale,
-		       (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius);
-
-		char buf[52];
-		sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
-			(double)mscale.y_offset, (double)mscale.z_offset);
-		mavlink_log_info(mavlink_fd, buf);
-
-		sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
-			(double)mscale.y_scale, (double)mscale.z_scale);
-		mavlink_log_info(mavlink_fd, buf);
-
-		mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
-		mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
-
-		close(sub_mag);
-		return OK;
-		/* third beep by cal end routine */
-
-	} else {
-		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
-		close(sub_mag);
-		return ERROR;
+		return res;
 	}
 }

From 495073935e428d99833135bb227f3085d2def1cf Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Mon, 21 Oct 2013 23:33:01 +0200
Subject: [PATCH 24/47] accelerometer_calibration: stability fix

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

diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index 49a8d6b339..5eeca5a1a3 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -180,10 +180,13 @@ int do_accel_calibration(int mavlink_fd)
 		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
 	}
 
-	/* measure and calculate offsets & scales */
 	float accel_offs[3];
 	float accel_T[3][3];
-	res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
+
+	if (res == OK) {
+		/* measure and calculate offsets & scales */
+		res = do_accel_calibration_measurements(mavlink_fd, accel_offs, accel_T);
+	}
 
 	if (res == OK) {
 		/* measurements completed successfully, rotate calibration values */

From 7f0ced968e5d518776930296ed870a47cc8c1756 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Mon, 21 Oct 2013 21:28:26 -0400
Subject: [PATCH 25/47] Working on roboclaw driver.

---
 makefiles/config_px4fmu-v1_default.mk  |   2 +-
 makefiles/config_px4fmu-v2_default.mk  |   1 +
 src/drivers/roboclaw/RoboClaw.cpp      | 250 +++++++++++++++++++++++++
 src/drivers/roboclaw/RoboClaw.hpp      | 187 ++++++++++++++++++
 src/drivers/roboclaw/module.mk         |  41 ++++
 src/drivers/roboclaw/roboclaw_main.cpp | 181 ++++++++++++++++++
 6 files changed, 661 insertions(+), 1 deletion(-)
 create mode 100644 src/drivers/roboclaw/RoboClaw.cpp
 create mode 100644 src/drivers/roboclaw/RoboClaw.hpp
 create mode 100644 src/drivers/roboclaw/module.mk
 create mode 100644 src/drivers/roboclaw/roboclaw_main.cpp

diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 46640f3c51..3cccc8447a 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -33,7 +33,7 @@ MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
 MODULES		+= drivers/rgbled
 MODULES		+= drivers/mkblctrl
-MODULES		+= drivers/md25
+MODULES		+= drivers/roboclaw
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index bb589cb9f3..a2027ded9b 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -31,6 +31,7 @@ MODULES		+= drivers/hil
 MODULES		+= drivers/hott/hott_telemetry
 MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
+MODULES		+= drivers/roboclaw
 MODULES		+= drivers/airspeed
 MODULES		+= drivers/ets_airspeed
 MODULES		+= drivers/meas_airspeed
diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
new file mode 100644
index 0000000000..88b22e72aa
--- /dev/null
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -0,0 +1,250 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RoboClaw.cpp
+ *
+ * RoboClaw Motor Driver
+ *
+ * references:
+ * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
+ *
+ */
+
+#include "RoboClaw.hpp"
+#include <poll.h>
+#include <stdio.h>
+#include <math.h>
+#include <string.h>
+#include <fcntl.h>
+#include <termios.h>
+
+#include <systemlib/err.h>
+#include <arch/board/board.h>
+#include <mavlink/mavlink_log.h>
+
+#include <controllib/uorb/UOrbPublication.hpp>
+#include <uORB/topics/debug_key_value.h>
+#include <drivers/drv_hrt.h>
+
+uint8_t RoboClaw::checksum_mask = 0x7f;
+
+RoboClaw::RoboClaw(const char *deviceName, uint16_t address):
+	_address(address),
+	_uart(0),
+	_controlPoll(),
+	_actuators(NULL, ORB_ID(actuator_controls_0), 20),
+	_motor1Position(0),
+	_motor1Speed(0),
+	_motor2Position(0),
+	_motor2Speed(0)
+{
+	// setup control polling
+	_controlPoll.fd = _actuators.getHandle();
+	_controlPoll.events = POLLIN;
+
+	// start serial port
+	_uart = open(deviceName, O_RDWR | O_NOCTTY);
+	struct termios uart_config;
+	tcgetattr(_uart, &uart_config);
+	uart_config.c_oflag &= ~ONLCR; // no CR for every LF
+	cfsetispeed(&uart_config, B38400);
+	cfsetospeed(&uart_config, B38400);
+	tcsetattr(_uart, TCSANOW, &uart_config);
+
+	// setup default settings, reset encoders
+	resetEncoders();
+}
+
+RoboClaw::~RoboClaw()
+{
+	setMotorDutyCycle(MOTOR_1, 0.0);
+	setMotorDutyCycle(MOTOR_2, 0.0);
+	close(_uart);
+}
+
+int RoboClaw::readEncoder(e_motor motor)
+{
+	uint16_t sum = 0;
+	if (motor == MOTOR_1) {
+		_sendCommand(CMD_READ_ENCODER_1, nullptr, 0, sum);
+	} else if (motor == MOTOR_2) {
+		_sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum);
+	}
+	uint8_t rbuf[6];
+	int ret = read(_uart, rbuf, 6);
+	uint32_t count = 0;
+	uint8_t * countBytes = (uint8_t *)(&count);
+	countBytes[3] = rbuf[0];
+	countBytes[2] = rbuf[1];
+	countBytes[1] = rbuf[2];
+	countBytes[0] = rbuf[3];
+	uint8_t status = rbuf[4];
+	if ((sum + _sumBytes(rbuf,5)) & 
+			checksum_mask == rbuf[5]) return -1;
+	int overFlow = 0;
+	if (status & STATUS_UNDERFLOW) {
+		overFlow = -1;
+	} else if (status & STATUS_OVERFLOW) {
+		overFlow = +1;
+	}
+	if (motor == MOTOR_1) {
+		_motor1Overflow += overFlow;
+	} else if (motor == MOTOR_2) {
+		_motor2Overflow += overFlow;
+	}
+	return ret;
+}
+
+void RoboClaw::status(char *string, size_t n)
+{
+	snprintf(string, n,
+		 "motor 1 position:\t%10.2f\tspeed:\t%10.2f\n"
+		 "motor 2 position:\t%10.2f\tspeed:\t%10.2f\n",
+		 double(getMotorPosition(MOTOR_1)),
+		 double(getMotorSpeed(MOTOR_1)),
+		 double(getMotorPosition(MOTOR_2)),
+		 double(getMotorSpeed(MOTOR_2)));
+}
+
+float RoboClaw::getMotorPosition(e_motor motor)
+{
+	if (motor == MOTOR_1) {
+		return _motor1Position;
+	} else if (motor == MOTOR_2) {
+		return _motor2Position;
+	}
+}
+
+float RoboClaw::getMotorSpeed(e_motor motor)
+{
+	if (motor == MOTOR_1) {
+		return _motor1Speed;
+	} else if (motor == MOTOR_2) {
+		return _motor2Speed;
+	}
+}
+
+int RoboClaw::setMotorSpeed(e_motor motor, float value)
+{
+	uint16_t sum = 0;
+	// bound
+	if (value > 1) value = 1;
+	if (value < -1) value = -1;
+	uint8_t speed = fabs(value)*127;
+	// send command
+	if (motor == MOTOR_1) {
+		if (value > 0) {
+			_sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
+		} else {
+			_sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
+		}
+	} else if (motor == MOTOR_2) {
+		if (value > 0) {
+			_sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
+		} else {
+			_sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
+		}
+	}
+}
+
+int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
+{
+	uint16_t sum = 0;
+	// bound
+	if (value > 1) value = 1;
+	if (value < -1) value = -1;
+	int16_t duty = 1500*value;
+	// send command
+	if (motor == MOTOR_1) {
+		_sendCommand(CMD_SIGNED_DUTYCYCLE_1,
+				(uint8_t *)(&duty), 2, sum);
+	} else if (motor == MOTOR_2) {
+		_sendCommand(CMD_SIGNED_DUTYCYCLE_2,
+				(uint8_t *)(&duty), 2, sum);
+	}
+}
+
+void RoboClaw::update()
+{
+	// wait for an actuator publication,
+	// check for exit condition every second
+	// note "::poll" is required to distinguish global
+	// poll from member function for driver
+	if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error
+
+	// if new data, send to motors
+	if (_actuators.updated()) {
+		_actuators.update();
+		setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]);
+		setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]);
+	}
+}
+
+uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
+{
+	uint16_t sum = 0;
+	for (size_t i=0;i<n;i++) {
+		sum += buf[i];
+	}
+	return sum;
+}
+
+int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, 
+		size_t n_data, uint16_t & prev_sum)
+{
+	tcflush(_uart, TCIOFLUSH);
+	uint8_t buf[n_data + 3];
+	buf[0] = _address;
+	buf[1] = cmd;
+	for (int i=0;i<n_data;i++) {
+		buf[i+2] = data[n_data - i - 1]; // MSB
+	}
+	uint16_t sum = _sumBytes(buf, n_data + 2);
+	prev_sum += sum;
+	buf[n_data + 2] = sum & checksum_mask;
+	return write(_uart, buf, n_data + 3);
+}
+
+int roboclawTest(const char *deviceName, uint8_t address)
+{
+	printf("roboclaw test: starting\n");
+
+	// setup
+	RoboClaw roboclaw(deviceName, address);
+
+	printf("Test complete\n");
+	return 0;
+}
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp
new file mode 100644
index 0000000000..27095b357d
--- /dev/null
+++ b/src/drivers/roboclaw/RoboClaw.hpp
@@ -0,0 +1,187 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file RoboClas.hpp
+ *
+ * RoboClaw Motor Driver
+ *
+ * references:
+ * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
+ *
+ */
+
+#pragma once
+
+#include <poll.h>
+#include <stdio.h>
+#include <controllib/uorb/UOrbSubscription.hpp>
+#include <uORB/topics/actuator_controls.h>
+#include <drivers/device/i2c.h>
+
+/**
+ * This is a driver for the RoboClaw motor controller
+ */
+class RoboClaw
+{
+public:
+
+	/** control channels */
+	enum e_channel {
+		CH_VOLTAGE_LEFT = 0,
+		CH_VOLTAGE_RIGHT
+	};
+	
+	/**  motors */
+	enum e_motor {
+		MOTOR_1 = 0,
+		MOTOR_2
+	};
+
+	/**
+	 * constructor
+	 * @param deviceName the name of the 
+	 * 	serial port e.g. "/dev/ttyS2"
+	 * @param address the adddress  of the motor
+	 * 	(selectable on roboclaw)
+	 */
+	RoboClaw(const char *deviceName, uint16_t address);
+
+	/**
+	 * deconstructor
+	 */
+	virtual ~RoboClaw();
+
+	/**
+	 * @return position of a motor, rev
+	 */
+	float getMotorPosition(e_motor motor);
+
+	/**
+	 * @return speed of a motor, rev/sec
+	 */
+	float getMotorSpeed(e_motor motor);
+
+	/**
+	 * set the speed of a motor, rev/sec
+	 */
+	int setMotorSpeed(e_motor motor, float value);
+
+	/**
+	 * set the duty cycle of a motor, rev/sec
+	 */
+	int setMotorDutyCycle(e_motor motor, float value);
+
+	/**
+	 * reset the encoders
+	 * @return status
+	 */
+	int resetEncoders();
+
+	/**
+	 * main update loop that updates RoboClaw motor
+	 * dutycycle based on actuator publication
+	 */
+	void update();
+
+	/**
+	 * read data from serial
+	 */
+	int readEncoder(e_motor motor);
+
+	/**
+	 * print status
+	 */
+	void status(char *string, size_t n);
+
+private:
+
+	// Quadrature status flags
+	enum e_quadrature_status_flags {
+		STATUS_UNDERFLOW = 1 << 0, /**< encoder went below 0 **/
+		STATUS_REVERSE = 1 << 1, /**< motor doing in reverse dir **/
+		STATUS_OVERFLOW = 1 << 2, /**< encoder went above 2^32 **/
+	};
+
+	// commands
+	// We just list the commands we want from the manual here.
+	enum e_command {
+
+		// simple
+		CMD_DRIVE_FWD_1 = 0,
+		CMD_DRIVE_REV_1 = 1,
+		CMD_DRIVE_FWD_2 = 4,
+		CMD_DRIVE_REV_2 = 5,
+
+		// encoder commands
+		CMD_READ_ENCODER_1 = 16,
+		CMD_READ_ENCODER_2 = 17,
+		CMD_RESET_ENCODERS = 20,
+
+		// advanced motor control
+		CMD_READ_SPEED_HIRES_1 = 30,
+		CMD_READ_SPEED_HIRES_2 = 31, 
+		CMD_SIGNED_DUTYCYCLE_1 = 32,
+		CMD_SIGNED_DUTYCYCLE_2 = 33,
+	};
+
+	static uint8_t checksum_mask;
+
+	uint16_t _address;
+
+	int _uart;
+
+	/** poll structure for control packets */
+	struct pollfd _controlPoll;
+
+	/** actuator controls subscription */
+	control::UOrbSubscription<actuator_controls_s> _actuators;
+
+	// private data
+	float _motor1Position;
+	float _motor1Speed;
+	int16_t _motor1Overflow;
+
+	float _motor2Position;
+	float _motor2Speed;
+	int16_t _motor2Overflow;
+
+	// private methods
+	uint16_t _sumBytes(uint8_t * buf, size_t n);
+	int _sendCommand(e_command cmd, uint8_t * data, size_t n_data, uint16_t & prev_sum);
+};
+
+// unit testing
+int roboclawTest(const char *deviceName, uint8_t address);
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk
new file mode 100644
index 0000000000..1abecf198b
--- /dev/null
+++ b/src/drivers/roboclaw/module.mk
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# RoboClaw Motor Controller
+#
+
+MODULE_COMMAND		= roboclaw
+
+SRCS			= roboclaw_main.cpp \
+			  RoboClaw.cpp
diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp
new file mode 100644
index 0000000000..a61c646fcf
--- /dev/null
+++ b/src/drivers/roboclaw/roboclaw_main.cpp
@@ -0,0 +1,181 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+
+
+/**
+ * @ file roboclaw_main.cpp
+ *
+ * RoboClaw Motor Driver
+ *
+ * references:
+ * http://downloads.orionrobotics.com/downloads/datasheets/motor_controller_robo_claw_R0401.pdf
+ *
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <math.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+
+#include <arch/board/board.h>
+#include "RoboClaw.hpp"
+
+static bool thread_should_exit = false;     /**< Deamon exit flag */
+static bool thread_running = false;     /**< Deamon status flag */
+static int deamon_task;             /**< Handle of deamon task / thread */
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int roboclaw_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of deamon.
+ */
+int roboclaw_thread_main(int argc, char *argv[]);
+
+/**
+ * Print the correct usage.
+ */
+static void usage();
+
+static void usage()
+{
+	fprintf(stderr, "usage: roboclaw "
+			"{start|stop|status|test}\n\n");
+}
+
+/**
+ * The deamon app only briefly exists to start
+ * the background job. The stack size assigned in the
+ * Makefile does only apply to this management task.
+ *
+ * The actual stack size should be set in the call
+ * to task_create().
+ */
+int roboclaw_main(int argc, char *argv[])
+{
+
+	if (argc < 1)
+		usage();
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			printf("roboclaw already running\n");
+			/* this is not an error */
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn_cmd("roboclaw",
+					 SCHED_DEFAULT,
+					 SCHED_PRIORITY_MAX - 10,
+					 2048,
+					 roboclaw_thread_main,
+					 (const char **)argv);
+		exit(0);
+
+	} else if (!strcmp(argv[1], "test")) {
+
+		if (argc < 4) {
+			printf("usage: roboclaw test device address\n");
+			exit(-1);
+		}
+
+		const char *deviceName = argv[1];
+		uint8_t address = strtoul(argv[2], nullptr, 0);
+
+		roboclawTest(deviceName, address);
+		thread_should_exit = true;
+		exit(0);
+
+	} else if (!strcmp(argv[1], "stop")) {
+
+		thread_should_exit = true;
+		exit(0);
+
+	} else if (!strcmp(argv[1], "status")) {
+
+		if (thread_running) {
+			printf("\troboclaw app is running\n");
+
+		} else {
+			printf("\troboclaw app not started\n");
+		}
+		exit(0);
+	}
+
+	usage();
+	exit(1);
+}
+
+int roboclaw_thread_main(int argc, char *argv[])
+{
+	printf("[roboclaw] starting\n");
+
+	// skip parent process args
+	argc -=2;
+	argv +=2;
+
+	if (argc < 3) {
+		printf("usage: roboclaw start device address\n");
+		return -1;
+	}
+
+	const char *deviceName = argv[1];
+	uint8_t address = strtoul(argv[2], nullptr, 0);
+
+	// start
+	RoboClaw roboclaw(deviceName, address);
+
+	thread_running = true;
+
+	// loop
+	while (!thread_should_exit) {
+		roboclaw.update();
+	}
+
+	// exit
+	printf("[roboclaw] exiting.\n");
+	thread_running = false;
+	return 0;
+}
+
+// vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78

From ce68f93867abfd3ea528809144f7c045d9bce544 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Mon, 21 Oct 2013 23:40:36 -0400
Subject: [PATCH 26/47] Debugging roboclaw comm.

---
 src/drivers/roboclaw/RoboClaw.cpp      | 52 ++++++++++++++++++++++----
 src/drivers/roboclaw/RoboClaw.hpp      |  2 +-
 src/drivers/roboclaw/roboclaw_main.cpp |  8 +++-
 3 files changed, 52 insertions(+), 10 deletions(-)

diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index 88b22e72aa..aecc511aed 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -75,12 +75,18 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address):
 
 	// start serial port
 	_uart = open(deviceName, O_RDWR | O_NOCTTY);
+	if (_uart < 0) err(1, "could not open %s", deviceName);
+	int ret = 0;
 	struct termios uart_config;
-	tcgetattr(_uart, &uart_config);
+	ret = tcgetattr(_uart, &uart_config);
+	if (ret < 0) err (1, "failed to get attr");
 	uart_config.c_oflag &= ~ONLCR; // no CR for every LF
-	cfsetispeed(&uart_config, B38400);
-	cfsetospeed(&uart_config, B38400);
-	tcsetattr(_uart, TCSANOW, &uart_config);
+	ret = cfsetispeed(&uart_config, B38400);
+	if (ret < 0) err (1, "failed to set input speed");
+	ret = cfsetospeed(&uart_config, B38400);
+	if (ret < 0) err (1, "failed to set output speed");
+	ret = tcsetattr(_uart, TCSANOW, &uart_config);
+	if (ret < 0) err (1, "failed to set attr");
 
 	// setup default settings, reset encoders
 	resetEncoders();
@@ -110,23 +116,30 @@ int RoboClaw::readEncoder(e_motor motor)
 	countBytes[1] = rbuf[2];
 	countBytes[0] = rbuf[3];
 	uint8_t status = rbuf[4];
-	if ((sum + _sumBytes(rbuf,5)) & 
-			checksum_mask == rbuf[5]) return -1;
+	if (((sum + _sumBytes(rbuf,5)) & checksum_mask) 
+			== rbuf[5]) return -1;
 	int overFlow = 0;
 	if (status & STATUS_UNDERFLOW) {
+		printf("roboclaw: underflow\n");
 		overFlow = -1;
 	} else if (status & STATUS_OVERFLOW) {
+		printf("roboclaw: overflow\n");
 		overFlow = +1;
 	}
+	static int64_t overflowAmount = 0x100000000LL;
 	if (motor == MOTOR_1) {
 		_motor1Overflow += overFlow;
+		_motor1Position = count + 
+			_motor1Overflow*_motor1Position;
 	} else if (motor == MOTOR_2) {
 		_motor2Overflow += overFlow;
+		_motor2Position = count + 
+			_motor2Overflow*_motor2Position;
 	}
 	return ret;
 }
 
-void RoboClaw::status(char *string, size_t n)
+void RoboClaw::printStatus(char *string, size_t n)
 {
 	snprintf(string, n,
 		 "motor 1 position:\t%10.2f\tspeed:\t%10.2f\n"
@@ -195,6 +208,13 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
 	}
 }
 
+int RoboClaw::resetEncoders() 
+{
+	uint16_t sum = 0;
+	return _sendCommand(CMD_RESET_ENCODERS,
+			nullptr, 0, sum);
+}
+
 void RoboClaw::update()
 {
 	// wait for an actuator publication,
@@ -214,9 +234,13 @@ void RoboClaw::update()
 uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
 {
 	uint16_t sum = 0;
+	printf("sum\n");
 	for (size_t i=0;i<n;i++) {
 		sum += buf[i];
+		printf("%d\t", buf[i]);
 	}
+	printf("total sum %d\n", sum);
+	printf("\n");
 	return sum;
 }
 
@@ -233,6 +257,11 @@ int RoboClaw::_sendCommand(e_command cmd, uint8_t * data,
 	uint16_t sum = _sumBytes(buf, n_data + 2);
 	prev_sum += sum;
 	buf[n_data + 2] = sum & checksum_mask;
+	printf("\nmessage:\n");
+	for (int i=0;i<n_data+3;i++) {
+		printf("%d\t", buf[i]);
+	}
+	printf("\n");
 	return write(_uart, buf, n_data + 3);
 }
 
@@ -242,6 +271,15 @@ int roboclawTest(const char *deviceName, uint8_t address)
 
 	// setup
 	RoboClaw roboclaw(deviceName, address);
+	char buf[200];
+	for (int i=0; i<10; i++) {
+		usleep(1000000);
+		roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
+		//roboclaw.setMotorSpeed(RoboClaw::MOTOR_1, 0.3);
+		//roboclaw.readEncoder(RoboClaw::MOTOR_1);
+		roboclaw.printStatus(buf, 200);
+		printf("%s\n", buf);
+	}
 
 	printf("Test complete\n");
 	return 0;
diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp
index 27095b357d..619c2289c9 100644
--- a/src/drivers/roboclaw/RoboClaw.hpp
+++ b/src/drivers/roboclaw/RoboClaw.hpp
@@ -122,7 +122,7 @@ public:
 	/**
 	 * print status
 	 */
-	void status(char *string, size_t n);
+	void printStatus(char *string, size_t n);
 
 private:
 
diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp
index a61c646fcf..f37f71aacc 100644
--- a/src/drivers/roboclaw/roboclaw_main.cpp
+++ b/src/drivers/roboclaw/roboclaw_main.cpp
@@ -119,8 +119,10 @@ int roboclaw_main(int argc, char *argv[])
 			exit(-1);
 		}
 
-		const char *deviceName = argv[1];
-		uint8_t address = strtoul(argv[2], nullptr, 0);
+		const char *deviceName = argv[2];
+		uint8_t address = strtoul(argv[3], nullptr, 0);
+
+		printf("device:\t%s\taddress:\t%d\n", deviceName, address);
 
 		roboclawTest(deviceName, address);
 		thread_should_exit = true;
@@ -162,6 +164,8 @@ int roboclaw_thread_main(int argc, char *argv[])
 	const char *deviceName = argv[1];
 	uint8_t address = strtoul(argv[2], nullptr, 0);
 
+	printf("device:\t%s\taddress:\t%d\n", deviceName, address);
+
 	// start
 	RoboClaw roboclaw(deviceName, address);
 

From 174c86321cad84589f597cfad1e5f7d6dd3c9136 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 22 Oct 2013 05:04:13 -0400
Subject: [PATCH 27/47] Roboclaw encoders/ dutycycledrive complete.

---
 ROMFS/px4fmu_common/init.d/40_io_segway |  16 +--
 ROMFS/px4fmu_common/init.d/rcS          |   6 ++
 src/drivers/roboclaw/RoboClaw.cpp       | 131 ++++++++++++++++--------
 src/drivers/roboclaw/RoboClaw.hpp       |  11 +-
 src/drivers/roboclaw/roboclaw_main.cpp  |  28 +++--
 5 files changed, 130 insertions(+), 62 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway
index 4b7ed52869..ffe7f695b1 100644
--- a/ROMFS/px4fmu_common/init.d/40_io_segway
+++ b/ROMFS/px4fmu_common/init.d/40_io_segway
@@ -21,8 +21,8 @@ param set MAV_TYPE 10
 #
 # Start MAVLink (depends on orb)
 #
-mavlink start -d /dev/ttyS1 -b 57600
-usleep 5000
+#mavlink start -d /dev/ttyS1 -b 57600
+#usleep 5000
  
 #
 # Start and configure PX4IO interface
@@ -32,25 +32,25 @@ sh /etc/init.d/rc.io
 #
 # Start the commander (depends on orb, mavlink)
 #
-commander start
+#commander start
 
 #
 # Start the sensors (depends on orb, px4io)
 #
-sh /etc/init.d/rc.sensors
+#sh /etc/init.d/rc.sensors
  
 #
 # Start GPS interface (depends on orb)
 #
-gps start
+#gps start
  
 #
 # Start the attitude estimator (depends on orb)
 #
-attitude_estimator_ekf start
+#attitude_estimator_ekf start
  
 #
 # Load mixer and start controllers (depends on px4io)
 #
-md25 start 3 0x58
-segway start
+roboclaw test /dev/ttyS2 128
+#segway start
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index fd588017f9..3a458286ed 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -272,6 +272,12 @@ then
 		sh /etc/init.d/30_io_camflyer
 		set MODE custom
 	fi
+
+	if param compare SYS_AUTOSTART 40
+	then
+		sh /etc/init.d/40_io_segway
+		set MODE custom
+	fi
 	
 	if param compare SYS_AUTOSTART 31
 	then
diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index aecc511aed..3bbd7f48f5 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -59,15 +59,19 @@
 
 uint8_t RoboClaw::checksum_mask = 0x7f;
 
-RoboClaw::RoboClaw(const char *deviceName, uint16_t address):
+RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
+		uint16_t pulsesPerRev):
 	_address(address),
+	_pulsesPerRev(pulsesPerRev),
 	_uart(0),
 	_controlPoll(),
 	_actuators(NULL, ORB_ID(actuator_controls_0), 20),
 	_motor1Position(0),
 	_motor1Speed(0),
+	_motor1Overflow(0),
 	_motor2Position(0),
-	_motor2Speed(0)
+	_motor2Speed(0),
+	_motor2Overflow(0)
 {
 	// setup control polling
 	_controlPoll.fd = _actuators.getHandle();
@@ -90,6 +94,13 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address):
 
 	// setup default settings, reset encoders
 	resetEncoders();
+
+	// XXX roboclaw gives 128 as first several csums
+	// have to read a couple of messages first
+	for (int i=0;i<5;i++) {
+		if (readEncoder(MOTOR_1) > 0) break;
+		usleep(3000);
+	}	
 }
 
 RoboClaw::~RoboClaw()
@@ -107,8 +118,18 @@ int RoboClaw::readEncoder(e_motor motor)
 	} else if (motor == MOTOR_2) {
 		_sendCommand(CMD_READ_ENCODER_2, nullptr, 0, sum);
 	}
-	uint8_t rbuf[6];
-	int ret = read(_uart, rbuf, 6);
+	uint8_t rbuf[50];
+	usleep(5000);
+	int nread = read(_uart, rbuf, 50);
+	if (nread < 6) { 
+		printf("failed to read\n");
+		return -1;
+	}
+	//printf("received: \n");
+	//for (int i=0;i<nread;i++) {
+		//printf("%d\t", rbuf[i]);
+	//}
+	//printf("\n");
 	uint32_t count = 0;
 	uint8_t * countBytes = (uint8_t *)(&count);
 	countBytes[3] = rbuf[0];
@@ -116,34 +137,46 @@ int RoboClaw::readEncoder(e_motor motor)
 	countBytes[1] = rbuf[2];
 	countBytes[0] = rbuf[3];
 	uint8_t status = rbuf[4];
-	if (((sum + _sumBytes(rbuf,5)) & checksum_mask) 
-			== rbuf[5]) return -1;
+	uint8_t checksum = rbuf[5];
+	uint8_t checksum_computed = (sum + _sumBytes(rbuf, 5)) & 
+		checksum_mask;
+	// check if checksum is valid
+	if (checksum != checksum_computed) {
+		printf("checksum failed: expected %d got %d\n",
+				checksum, checksum_computed);
+		return -1;
+	}
 	int overFlow = 0;
+
+	if (status & STATUS_REVERSE) {
+		//printf("roboclaw: reverse\n");
+	}
+
 	if (status & STATUS_UNDERFLOW) {
-		printf("roboclaw: underflow\n");
+		//printf("roboclaw: underflow\n");
 		overFlow = -1;
 	} else if (status & STATUS_OVERFLOW) {
-		printf("roboclaw: overflow\n");
+		//printf("roboclaw: overflow\n");
 		overFlow = +1;
 	}
+
 	static int64_t overflowAmount = 0x100000000LL;
 	if (motor == MOTOR_1) {
 		_motor1Overflow += overFlow;
-		_motor1Position = count + 
-			_motor1Overflow*_motor1Position;
+		_motor1Position = float(int64_t(count) + 
+			_motor1Overflow*overflowAmount)/_pulsesPerRev;
 	} else if (motor == MOTOR_2) {
 		_motor2Overflow += overFlow;
-		_motor2Position = count + 
-			_motor2Overflow*_motor2Position;
+		_motor2Position = float(int64_t(count) + 
+			_motor2Overflow*overflowAmount)/_pulsesPerRev;
 	}
-	return ret;
+	return 0;
 }
 
 void RoboClaw::printStatus(char *string, size_t n)
 {
 	snprintf(string, n,
-		 "motor 1 position:\t%10.2f\tspeed:\t%10.2f\n"
-		 "motor 2 position:\t%10.2f\tspeed:\t%10.2f\n",
+		 "pos1,spd1,pos2,spd2: %10.2f %10.2f %10.2f %10.2f\n",
 		 double(getMotorPosition(MOTOR_1)),
 		 double(getMotorSpeed(MOTOR_1)),
 		 double(getMotorPosition(MOTOR_2)),
@@ -178,17 +211,18 @@ int RoboClaw::setMotorSpeed(e_motor motor, float value)
 	// send command
 	if (motor == MOTOR_1) {
 		if (value > 0) {
-			_sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
+			return _sendCommand(CMD_DRIVE_FWD_1, &speed, 1, sum);
 		} else {
-			_sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
+			return _sendCommand(CMD_DRIVE_REV_1, &speed, 1, sum);
 		}
 	} else if (motor == MOTOR_2) {
 		if (value > 0) {
-			_sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
+			return _sendCommand(CMD_DRIVE_FWD_2, &speed, 1, sum);
 		} else {
-			_sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
+			return _sendCommand(CMD_DRIVE_REV_2, &speed, 1, sum);
 		}
 	}
+	return -1;
 }
 
 int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
@@ -200,12 +234,13 @@ int RoboClaw::setMotorDutyCycle(e_motor motor, float value)
 	int16_t duty = 1500*value;
 	// send command
 	if (motor == MOTOR_1) {
-		_sendCommand(CMD_SIGNED_DUTYCYCLE_1,
+		return _sendCommand(CMD_SIGNED_DUTYCYCLE_1,
 				(uint8_t *)(&duty), 2, sum);
 	} else if (motor == MOTOR_2) {
-		_sendCommand(CMD_SIGNED_DUTYCYCLE_2,
+		return _sendCommand(CMD_SIGNED_DUTYCYCLE_2,
 				(uint8_t *)(&duty), 2, sum);
 	}
+	return -1;
 }
 
 int RoboClaw::resetEncoders() 
@@ -215,13 +250,13 @@ int RoboClaw::resetEncoders()
 			nullptr, 0, sum);
 }
 
-void RoboClaw::update()
+int RoboClaw::update()
 {
 	// wait for an actuator publication,
 	// check for exit condition every second
 	// note "::poll" is required to distinguish global
 	// poll from member function for driver
-	if (::poll(&_controlPoll, 1, 1000) < 0) return; // poll error
+	if (::poll(&_controlPoll, 1, 1000) < 0) return -1; // poll error
 
 	// if new data, send to motors
 	if (_actuators.updated()) {
@@ -229,56 +264,68 @@ void RoboClaw::update()
 		setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]);
 		setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]);
 	}
+	return 0;
 }
 
 uint16_t RoboClaw::_sumBytes(uint8_t * buf, size_t n)
 {
 	uint16_t sum = 0;
-	printf("sum\n");
+	//printf("sum\n");
 	for (size_t i=0;i<n;i++) {
 		sum += buf[i];
-		printf("%d\t", buf[i]);
+		//printf("%d\t", buf[i]);
 	}
-	printf("total sum %d\n", sum);
-	printf("\n");
+	//printf("total sum %d\n", sum);
 	return sum;
 }
 
 int RoboClaw::_sendCommand(e_command cmd, uint8_t * data, 
 		size_t n_data, uint16_t & prev_sum)
 {
-	tcflush(_uart, TCIOFLUSH);
+	tcflush(_uart, TCIOFLUSH); // flush  buffers
 	uint8_t buf[n_data + 3];
 	buf[0] = _address;
 	buf[1] = cmd;
-	for (int i=0;i<n_data;i++) {
+	for (size_t i=0;i<n_data;i++) {
 		buf[i+2] = data[n_data - i - 1]; // MSB
 	}
 	uint16_t sum = _sumBytes(buf, n_data + 2);
 	prev_sum += sum;
 	buf[n_data + 2] = sum & checksum_mask;
-	printf("\nmessage:\n");
-	for (int i=0;i<n_data+3;i++) {
-		printf("%d\t", buf[i]);
-	}
-	printf("\n");
+	//printf("\nmessage:\n");
+	//for (size_t i=0;i<n_data+3;i++) {
+		//printf("%d\t", buf[i]);
+	//}
+	//printf("\n");
 	return write(_uart, buf, n_data + 3);
 }
 
-int roboclawTest(const char *deviceName, uint8_t address)
+int roboclawTest(const char *deviceName, uint8_t address, 
+		uint16_t pulsesPerRev)
 {
 	printf("roboclaw test: starting\n");
 
 	// setup
-	RoboClaw roboclaw(deviceName, address);
+	RoboClaw roboclaw(deviceName, address, pulsesPerRev);
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, 0.3);
 	char buf[200];
 	for (int i=0; i<10; i++) {
-		usleep(1000000);
-		roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, 0.3);
-		//roboclaw.setMotorSpeed(RoboClaw::MOTOR_1, 0.3);
-		//roboclaw.readEncoder(RoboClaw::MOTOR_1);
-		roboclaw.printStatus(buf, 200);
-		printf("%s\n", buf);
+		usleep(100000);
+		roboclaw.readEncoder(RoboClaw::MOTOR_1);
+		roboclaw.readEncoder(RoboClaw::MOTOR_2);
+		roboclaw.printStatus(buf,200);
+		printf("%s", buf);
+	}
+
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_1, -0.3);
+	roboclaw.setMotorDutyCycle(RoboClaw::MOTOR_2, -0.3);
+	for (int i=0; i<10; i++) {
+		usleep(100000);
+		roboclaw.readEncoder(RoboClaw::MOTOR_1);
+		roboclaw.readEncoder(RoboClaw::MOTOR_2);
+		roboclaw.printStatus(buf,200);
+		printf("%s", buf);
 	}
 
 	printf("Test complete\n");
diff --git a/src/drivers/roboclaw/RoboClaw.hpp b/src/drivers/roboclaw/RoboClaw.hpp
index 619c2289c9..e9f35cf956 100644
--- a/src/drivers/roboclaw/RoboClaw.hpp
+++ b/src/drivers/roboclaw/RoboClaw.hpp
@@ -74,8 +74,11 @@ public:
 	 * 	serial port e.g. "/dev/ttyS2"
 	 * @param address the adddress  of the motor
 	 * 	(selectable on roboclaw)
+	 * @param pulsesPerRev # of encoder
+	 *  pulses per revolution of wheel
 	 */
-	RoboClaw(const char *deviceName, uint16_t address);
+	RoboClaw(const char *deviceName, uint16_t address, 
+			uint16_t pulsesPerRev);
 
 	/**
 	 * deconstructor
@@ -112,7 +115,7 @@ public:
 	 * main update loop that updates RoboClaw motor
 	 * dutycycle based on actuator publication
 	 */
-	void update();
+	int update();
 
 	/**
 	 * read data from serial
@@ -158,6 +161,7 @@ private:
 	static uint8_t checksum_mask;
 
 	uint16_t _address;
+	uint16_t _pulsesPerRev;
 
 	int _uart;
 
@@ -182,6 +186,7 @@ private:
 };
 
 // unit testing
-int roboclawTest(const char *deviceName, uint8_t address);
+int roboclawTest(const char *deviceName, uint8_t address,
+		uint16_t pulsesPerRev);
 
 // vi:noet:smarttab:autoindent:ts=4:sw=4:tw=78
diff --git a/src/drivers/roboclaw/roboclaw_main.cpp b/src/drivers/roboclaw/roboclaw_main.cpp
index f37f71aacc..44ed03254f 100644
--- a/src/drivers/roboclaw/roboclaw_main.cpp
+++ b/src/drivers/roboclaw/roboclaw_main.cpp
@@ -114,17 +114,25 @@ int roboclaw_main(int argc, char *argv[])
 
 	} else if (!strcmp(argv[1], "test")) {
 
-		if (argc < 4) {
-			printf("usage: roboclaw test device address\n");
+		const char * deviceName = "/dev/ttyS2";
+		uint8_t address = 128;
+		uint16_t pulsesPerRev = 1200;
+
+		if (argc == 2) {
+			printf("testing with default settings\n");
+		} else if (argc != 4) {
+			printf("usage: roboclaw test device address pulses_per_rev\n");
 			exit(-1);
+		} else {
+			deviceName = argv[2];
+			address = strtoul(argv[3], nullptr, 0);
+			pulsesPerRev = strtoul(argv[4], nullptr, 0);
 		}
 
-		const char *deviceName = argv[2];
-		uint8_t address = strtoul(argv[3], nullptr, 0);
+		printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
+			deviceName, address, pulsesPerRev);
 
-		printf("device:\t%s\taddress:\t%d\n", deviceName, address);
-
-		roboclawTest(deviceName, address);
+		roboclawTest(deviceName, address, pulsesPerRev);
 		thread_should_exit = true;
 		exit(0);
 
@@ -163,11 +171,13 @@ int roboclaw_thread_main(int argc, char *argv[])
 
 	const char *deviceName = argv[1];
 	uint8_t address = strtoul(argv[2], nullptr, 0);
+	uint16_t pulsesPerRev = strtoul(argv[3], nullptr, 0);
 
-	printf("device:\t%s\taddress:\t%d\n", deviceName, address);
+	printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
+			deviceName, address, pulsesPerRev);
 
 	// start
-	RoboClaw roboclaw(deviceName, address);
+	RoboClaw roboclaw(deviceName, address, pulsesPerRev);
 
 	thread_running = true;
 

From d1a4dd240c9b55da72a109f553af2eb9ca2bef73 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 22 Oct 2013 05:08:20 -0400
Subject: [PATCH 28/47] Updated script.

---
 ROMFS/px4fmu_common/init.d/40_io_segway | 16 ++++++++--------
 ROMFS/px4fmu_common/init.d/rcS          | 12 ++++++------
 2 files changed, 14 insertions(+), 14 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/40_io_segway b/ROMFS/px4fmu_common/init.d/40_io_segway
index ffe7f695b1..fb9dec0437 100644
--- a/ROMFS/px4fmu_common/init.d/40_io_segway
+++ b/ROMFS/px4fmu_common/init.d/40_io_segway
@@ -21,8 +21,8 @@ param set MAV_TYPE 10
 #
 # Start MAVLink (depends on orb)
 #
-#mavlink start -d /dev/ttyS1 -b 57600
-#usleep 5000
+mavlink start -d /dev/ttyS1 -b 57600
+usleep 5000
  
 #
 # Start and configure PX4IO interface
@@ -32,25 +32,25 @@ sh /etc/init.d/rc.io
 #
 # Start the commander (depends on orb, mavlink)
 #
-#commander start
+commander start
 
 #
 # Start the sensors (depends on orb, px4io)
 #
-#sh /etc/init.d/rc.sensors
+sh /etc/init.d/rc.sensors
  
 #
 # Start GPS interface (depends on orb)
 #
-#gps start
+gps start
  
 #
 # Start the attitude estimator (depends on orb)
 #
-#attitude_estimator_ekf start
+attitude_estimator_ekf start
  
 #
 # Load mixer and start controllers (depends on px4io)
 #
-roboclaw test /dev/ttyS2 128
-#segway start
+roboclaw start /dev/ttyS2 128 1200
+segway start
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 3a458286ed..424787d54a 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -273,18 +273,18 @@ then
 		set MODE custom
 	fi
 
-	if param compare SYS_AUTOSTART 40
-	then
-		sh /etc/init.d/40_io_segway
-		set MODE custom
-	fi
-	
 	if param compare SYS_AUTOSTART 31
 	then
 		sh /etc/init.d/31_io_phantom
 		set MODE custom
 	fi
 
+	if param compare SYS_AUTOSTART 40
+	then
+		sh /etc/init.d/40_io_segway
+		set MODE custom
+	fi
+	
 	if param compare SYS_AUTOSTART 100
 	then
 		sh /etc/init.d/100_mpx_easystar

From 108d723a4902086f883136c7de6a75d65256500b Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 22 Oct 2013 05:10:26 -0400
Subject: [PATCH 29/47] Removed old timing hack.

---
 src/drivers/roboclaw/RoboClaw.cpp | 7 -------
 1 file changed, 7 deletions(-)

diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index 3bbd7f48f5..73bd5dadac 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -94,13 +94,6 @@ RoboClaw::RoboClaw(const char *deviceName, uint16_t address,
 
 	// setup default settings, reset encoders
 	resetEncoders();
-
-	// XXX roboclaw gives 128 as first several csums
-	// have to read a couple of messages first
-	for (int i=0;i<5;i++) {
-		if (readEncoder(MOTOR_1) > 0) break;
-		usleep(3000);
-	}	
 }
 
 RoboClaw::~RoboClaw()

From d143e827dcd774548bca6d6b4cb6fb69ef35a826 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 22 Oct 2013 05:43:10 -0400
Subject: [PATCH 30/47] Updated segway controller for new state machine.

---
 makefiles/config_px4fmu-v1_default.mk        |  2 +-
 makefiles/config_px4fmu-v2_default.mk        |  2 +-
 src/modules/segway/BlockSegwayController.cpp | 13 +++++++------
 3 files changed, 9 insertions(+), 8 deletions(-)

diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index 3cccc8447a..85ac3f546f 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -79,7 +79,7 @@ MODULES		+= examples/flow_position_estimator
 #
 # Vehicle Control
 #
-#MODULES		+= modules/segway # XXX needs state machine update
+MODULES		+= modules/segway
 MODULES		+= modules/fw_pos_control_l1
 MODULES		+= modules/fw_att_control
 MODULES		+= modules/multirotor_att_control
diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index a2027ded9b..c0972be9eb 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -78,7 +78,7 @@ MODULES		+= examples/flow_position_estimator
 #
 # Vehicle Control
 #
-#MODULES		+= modules/segway # XXX needs state machine update
+MODULES		+= modules/segway
 MODULES		+= modules/fw_pos_control_l1
 MODULES		+= modules/fw_att_control
 MODULES		+= modules/multirotor_att_control
diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp
index b1dc39445f..96a443c6ea 100644
--- a/src/modules/segway/BlockSegwayController.cpp
+++ b/src/modules/segway/BlockSegwayController.cpp
@@ -26,7 +26,7 @@ void BlockSegwayController::update() {
 		_actuators.control[i] = 0.0f;
 
 	// only update guidance in auto mode
-	if (_status.state_machine == SYSTEM_STATE_AUTO) {
+	if (_status.main_state == MAIN_STATE_AUTO) {
 		// update guidance
 	}
 
@@ -34,17 +34,18 @@ void BlockSegwayController::update() {
 	float spdCmd = -th2v.update(_att.pitch) - q2v.update(_att.pitchspeed);
 
 	// handle autopilot modes
-	if (_status.state_machine == SYSTEM_STATE_AUTO ||
-	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
+	if (_status.main_state == MAIN_STATE_AUTO ||
+	    _status.main_state == MAIN_STATE_SEATBELT ||
+	    _status.main_state == MAIN_STATE_EASY) {
 		_actuators.control[0] = spdCmd;
 		_actuators.control[1] = spdCmd;
 
-	} else if (_status.state_machine == SYSTEM_STATE_MANUAL) {
-		if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) {
+	} else if (_status.main_state == MAIN_STATE_MANUAL) {
+		if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {
 			_actuators.control[CH_LEFT] = _manual.throttle;
 			_actuators.control[CH_RIGHT] = _manual.pitch;
 
-		} else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) {
+		} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {
 			_actuators.control[0] = spdCmd;
 			_actuators.control[1] = spdCmd;
 		}

From c4a1a338ff4378c908ec34c5a5f408c1b96d0735 Mon Sep 17 00:00:00 2001
From: James Goppert <james.goppert@gmail.com>
Date: Tue, 22 Oct 2013 05:43:27 -0400
Subject: [PATCH 31/47] Changed driver to control motor duty cycle.

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

diff --git a/src/drivers/roboclaw/RoboClaw.cpp b/src/drivers/roboclaw/RoboClaw.cpp
index 73bd5dadac..d65a9be361 100644
--- a/src/drivers/roboclaw/RoboClaw.cpp
+++ b/src/drivers/roboclaw/RoboClaw.cpp
@@ -254,8 +254,8 @@ int RoboClaw::update()
 	// if new data, send to motors
 	if (_actuators.updated()) {
 		_actuators.update();
-		setMotorSpeed(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]);
-		setMotorSpeed(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]);
+		setMotorDutyCycle(MOTOR_1,_actuators.control[CH_VOLTAGE_LEFT]);
+		setMotorDutyCycle(MOTOR_2,_actuators.control[CH_VOLTAGE_RIGHT]);
 	}
 	return 0;
 }

From 28b4e978534e164d08125a9b0cf1fe428d9ad122 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 22 Oct 2013 21:01:30 +0200
Subject: [PATCH 32/47] Fixed bug with fd leak in rc_calibration_check

---
 src/modules/commander/commander.cpp              | 4 ++--
 src/modules/systemlib/rc_check.c                 | 4 +---
 src/modules/systemlib/rc_check.h                 | 2 +-
 src/systemcmds/preflight_check/preflight_check.c | 4 ++--
 4 files changed, 6 insertions(+), 8 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 2ef509980c..db758c3865 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -687,7 +687,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	bool updated = false;
 
-	bool rc_calibration_ok = (OK == rc_calibration_check());
+	bool rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
 
 	/* Subscribe to safety topic */
 	int safety_sub = orb_subscribe(ORB_ID(safety));
@@ -802,7 +802,7 @@ int commander_thread_main(int argc, char *argv[])
 				status_changed = true;
 
 				/* re-check RC calibration */
-				rc_calibration_ok = (OK == rc_calibration_check());
+				rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
 
 				/* navigation parameters */
 				param_get(_param_takeoff_alt, &takeoff_alt);
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
index 60d6473b8c..b4350cc243 100644
--- a/src/modules/systemlib/rc_check.c
+++ b/src/modules/systemlib/rc_check.c
@@ -47,14 +47,12 @@
 #include <mavlink/mavlink_log.h>
 #include <uORB/topics/rc_channels.h>
 
-int rc_calibration_check(void) {
+int rc_calibration_check(int mavlink_fd) {
 
 	char nbuf[20];
 	param_t _parameter_handles_min, _parameter_handles_trim, _parameter_handles_max,
 	_parameter_handles_rev, _parameter_handles_dz;
 
-	int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
 	float param_min, param_max, param_trim, param_rev, param_dz;
 
 	/* first check channel mappings */
diff --git a/src/modules/systemlib/rc_check.h b/src/modules/systemlib/rc_check.h
index e2238d1516..e70b83ccee 100644
--- a/src/modules/systemlib/rc_check.h
+++ b/src/modules/systemlib/rc_check.h
@@ -47,6 +47,6 @@
  * @return			0 / OK if RC calibration is ok, index + 1 of the first
  *				channel that failed else (so 1 == first channel failed)
  */
-__EXPORT int	rc_calibration_check(void);
+__EXPORT int	rc_calibration_check(int mavlink_fd);
 
 __END_DECLS
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index e9c5f1a2cf..1c58a2db69 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -140,7 +140,7 @@ int preflight_check_main(int argc, char *argv[])
 
 	/* ---- RC CALIBRATION ---- */
 
-	bool rc_ok = (OK == rc_calibration_check());
+	bool rc_ok = (OK == rc_calibration_check(mavlink_fd));
 
 	/* warn */
 	if (!rc_ok)
@@ -227,4 +227,4 @@ static int led_off(int leds, int led)
 static int led_on(int leds, int led)
 {
 	return ioctl(leds, LED_ON, led);
-}
\ No newline at end of file
+}

From 2f66a8894f1f8035bfc076306aa0d83197be108a Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 22 Oct 2013 21:02:29 +0200
Subject: [PATCH 33/47] param_save_default() rewritten: don't try 10 times to
 do every operation but do it safe using temp file

---
 src/modules/systemlib/param/param.c | 95 ++++++++++++++---------------
 1 file changed, 47 insertions(+), 48 deletions(-)

diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index ccdb2ea38b..398657dd79 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -508,64 +508,63 @@ param_get_default_file(void)
 int
 param_save_default(void)
 {
-	int result;
-	unsigned retries = 0;
-
-	/* delete the file in case it exists */
-	struct stat buffer;
-	if (stat(param_get_default_file(), &buffer) == 0) {
-
-		do {
-			result = unlink(param_get_default_file());
-			if (result != 0) {
-				retries++;
-				usleep(1000 * retries);
-			}
-		} while (result != OK && retries < 10);
-
-		if (result != OK)
-			warnx("unlinking file %s failed.", param_get_default_file());
-	}
-
-	/* create the file */
+	int res;
 	int fd;
 
-	do {
-		/* do another attempt in case the unlink call is not synced yet */
-		fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
+	const char *filename = param_get_default_file();
+	const char *filename_tmp = malloc(strlen(filename) + 5);
+	sprintf(filename_tmp, "%s.tmp", filename);
+
+	/* delete temp file if exist */
+	res = unlink(filename_tmp);
+
+	if (res != OK && errno == ENOENT)
+		res = OK;
+
+	if (res != OK)
+		warn("failed to delete temp file: %s", filename_tmp);
+
+	if (res == OK) {
+		/* write parameters to temp file */
+		fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL);
+
 		if (fd < 0) {
-			retries++;
-			usleep(1000 * retries);
+			warn("failed to open temp file: %s", filename_tmp);
+			res = ERROR;
 		}
 
-	} while (fd < 0 && retries < 10);
+		if (res == OK) {
+			res = param_export(fd, false);
 
-	if (fd < 0) {
-		
-		warn("opening '%s' for writing failed", param_get_default_file());
-		return fd;
-	}
-
-	do {
-		result = param_export(fd, false);
-
-		if (result != OK) {
-			retries++;
-			usleep(1000 * retries);
+			if (res != OK)
+				warnx("failed to write parameters to file: %s", filename_tmp);
 		}
 
-	} while (result != 0 && retries < 10);
-
-
-	close(fd);
-
-	if (result != OK) {
-		warn("error exporting parameters to '%s'", param_get_default_file());
-		(void)unlink(param_get_default_file());
-		return result;
+		close(fd);
 	}
 
-	return 0;
+	if (res == OK) {
+		/* delete parameters file */
+		res = unlink(filename);
+
+		if (res != OK && errno == ENOENT)
+			res = OK;
+
+		if (res != OK)
+			warn("failed to delete parameters file: %s", filename);
+	}
+
+	if (res == OK) {
+		/* rename temp file to parameters */
+		res = rename(filename_tmp, filename);
+
+		if (res != OK)
+			warn("failed to rename %s to %s", filename_tmp, filename);
+	}
+
+	free(filename_tmp);
+
+	return res;
 }
 
 /**

From 3c6f43869178719abef90e5ef73e02dee952b1ce Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 23 Oct 2013 18:57:06 +0200
Subject: [PATCH 34/47] sdlog2: parameters logging implemented (APM-compatible)

---
 src/modules/sdlog2/sdlog2.c          | 122 +++++++++++++++++++--------
 src/modules/sdlog2/sdlog2_format.h   |   8 +-
 src/modules/sdlog2/sdlog2_messages.h |  27 ++++--
 3 files changed, 110 insertions(+), 47 deletions(-)

diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 61a54170a3..f94875d5b8 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -58,6 +58,7 @@
 #include <systemlib/err.h>
 #include <unistd.h>
 #include <drivers/drv_hrt.h>
+#include <math.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
@@ -84,6 +85,7 @@
 #include <uORB/topics/esc_status.h>
 
 #include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
 
 #include <mavlink/mavlink_log.h>
 
@@ -181,12 +183,17 @@ static void sdlog2_stop_log(void);
 /**
  * Write a header to log file: list of message formats.
  */
-static void write_formats(int fd);
+static int write_formats(int fd);
 
 /**
  * Write version message to log file.
  */
-static void write_version(int fd);
+static int write_version(int fd);
+
+/**
+ * Write parameters to log file.
+ */
+static int write_parameters(int fd);
 
 static bool file_exist(const char *filename);
 
@@ -242,11 +249,11 @@ int sdlog2_main(int argc, char *argv[])
 
 		main_thread_should_exit = false;
 		deamon_task = task_spawn_cmd("sdlog2",
-					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_DEFAULT - 30,
-					 3000,
-					 sdlog2_thread_main,
-					 (const char **)argv);
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_DEFAULT - 30,
+					     3000,
+					     sdlog2_thread_main,
+					     (const char **)argv);
 		exit(0);
 	}
 
@@ -359,13 +366,13 @@ static void *logwriter_thread(void *arg)
 
 	struct logbuffer_s *logbuf = (struct logbuffer_s *)arg;
 
-	int log_file = open_logfile();
+	int log_fd = open_logfile();
 
-	/* write log messages formats */
-	write_formats(log_file);
-
-	/* write version */
-	write_version(log_file);
+	/* write log messages formats, version and parameters */
+	log_bytes_written += write_formats(log_fd);
+	log_bytes_written += write_version(log_fd);
+	log_bytes_written += write_parameters(log_fd);
+	fsync(log_fd);
 
 	int poll_count = 0;
 
@@ -404,7 +411,7 @@ static void *logwriter_thread(void *arg)
 				n = available;
 			}
 
-			n = write(log_file, read_ptr, n);
+			n = write(log_fd, read_ptr, n);
 
 			should_wait = (n == available) && !is_part;
 
@@ -419,21 +426,23 @@ static void *logwriter_thread(void *arg)
 
 		} else {
 			n = 0;
+
 			/* exit only with empty buffer */
 			if (main_thread_should_exit || logwriter_should_exit) {
 				break;
 			}
+
 			should_wait = true;
 		}
 
 		if (++poll_count == 10) {
-			fsync(log_file);
+			fsync(log_fd);
 			poll_count = 0;
 		}
 	}
 
-	fsync(log_file);
-	close(log_file);
+	fsync(log_fd);
+	close(log_fd);
 
 	return OK;
 }
@@ -487,15 +496,17 @@ void sdlog2_stop_log()
 
 	/* wait for write thread to return */
 	int ret;
+
 	if ((ret = pthread_join(logwriter_pthread, NULL)) != 0) {
 		warnx("error joining logwriter thread: %i", ret);
 	}
+
 	logwriter_pthread = 0;
 
 	sdlog2_status();
 }
 
-void write_formats(int fd)
+int write_formats(int fd)
 {
 	/* construct message format packet */
 	struct {
@@ -505,35 +516,72 @@ void write_formats(int fd)
 		LOG_PACKET_HEADER_INIT(LOG_FORMAT_MSG),
 	};
 
-	/* fill message format packet for each format and write to log */
-	int i;
+	int written = 0;
 
-	for (i = 0; i < log_formats_num; i++) {
+	/* fill message format packet for each format and write it */
+	for (int i = 0; i < log_formats_num; i++) {
 		log_msg_format.body = log_formats[i];
-		log_bytes_written += write(fd, &log_msg_format, sizeof(log_msg_format));
+		written += write(fd, &log_msg_format, sizeof(log_msg_format));
 	}
 
-	fsync(fd);
+	return written;
 }
 
-void write_version(int fd)
+int write_version(int fd)
 {
 	/* construct version message */
 	struct {
 		LOG_PACKET_HEADER;
 		struct log_VER_s body;
 	} log_msg_VER = {
-		LOG_PACKET_HEADER_INIT(127),
+		LOG_PACKET_HEADER_INIT(LOG_VER_MSG),
 	};
 
-	/* fill message format packet for each format and write to log */
-	int i;
-
+	/* fill version message and write it */
 	strncpy(log_msg_VER.body.fw_git, FW_GIT, sizeof(log_msg_VER.body.fw_git));
 	strncpy(log_msg_VER.body.arch, HW_ARCH, sizeof(log_msg_VER.body.arch));
-	log_bytes_written += write(fd, &log_msg_VER, sizeof(log_msg_VER));
+	return write(fd, &log_msg_VER, sizeof(log_msg_VER));
+}
 
-	fsync(fd);
+int write_parameters(int fd)
+{
+	/* construct parameter message */
+	struct {
+		LOG_PACKET_HEADER;
+		struct log_PARM_s body;
+	} log_msg_PARM = {
+		LOG_PACKET_HEADER_INIT(LOG_PARM_MSG),
+	};
+
+	int written = 0;
+	param_t params_cnt = param_count();
+
+	for (param_t param = 0; param < params_cnt; param++) {
+		/* fill parameter message and write it */
+		strncpy(log_msg_PARM.body.name, param_name(param), sizeof(log_msg_PARM.body.name));
+		float value = NAN;
+
+		switch (param_type(param)) {
+		case PARAM_TYPE_INT32: {
+				int32_t i;
+				param_get(param, &i);
+				value = i;	// cast integer to float
+				break;
+			}
+
+		case PARAM_TYPE_FLOAT:
+				param_get(param, &value);
+				break;
+
+		default:
+			break;
+		}
+
+		log_msg_PARM.body.value = value;
+		written += write(fd, &log_msg_PARM, sizeof(log_msg_PARM));
+	}
+
+	return written;
 }
 
 int sdlog2_thread_main(int argc, char *argv[])
@@ -611,12 +659,13 @@ int sdlog2_thread_main(int argc, char *argv[])
 	}
 
 	const char *converter_in = "/etc/logging/conv.zip";
-	char* converter_out = malloc(120);
+	char *converter_out = malloc(120);
 	sprintf(converter_out, "%s/conv.zip", folder_path);
 
 	if (file_copy(converter_in, converter_out)) {
 		errx(1, "unable to copy conversion scripts, exiting.");
 	}
+
 	free(converter_out);
 
 	/* only print logging path, important to find log file later */
@@ -630,6 +679,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	}
 
 	struct vehicle_status_s buf_status;
+
 	memset(&buf_status, 0, sizeof(buf_status));
 
 	/* warning! using union here to save memory, elements should be used separately! */
@@ -655,6 +705,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct esc_status_s esc;
 		struct vehicle_global_velocity_setpoint_s global_vel_sp;
 	} buf;
+
 	memset(&buf, 0, sizeof(buf));
 
 	struct {
@@ -825,7 +876,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	fds[fdsc_count].fd = subs.airspeed_sub;
 	fds[fdsc_count].events = POLLIN;
-	fdsc_count++;	
+	fdsc_count++;
 
 	/* --- ESCs --- */
 	subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
@@ -913,7 +964,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 				continue;
 			}
 
-			ifds = 1;	// Begin from fds[1] again
+			ifds = 1;	// begin from fds[1] again
 
 			pthread_mutex_lock(&logbuffer_mutex);
 
@@ -1172,8 +1223,8 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* --- ESCs --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
-				for (uint8_t i=0; i<buf.esc.esc_count; i++)
-				{
+
+				for (uint8_t i = 0; i < buf.esc.esc_count; i++) {
 					log_msg.msg_type = LOG_ESC_MSG;
 					log_msg.body.log_ESC.counter = buf.esc.counter;
 					log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
@@ -1321,6 +1372,7 @@ void handle_status(struct vehicle_status_s *status)
 {
 	// TODO use flag from actuator_armed here?
 	bool armed = status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR;
+
 	if (armed != flag_system_armed) {
 		flag_system_armed = armed;
 
diff --git a/src/modules/sdlog2/sdlog2_format.h b/src/modules/sdlog2/sdlog2_format.h
index 5c175ef7ef..dc5e6c8bd4 100644
--- a/src/modules/sdlog2/sdlog2_format.h
+++ b/src/modules/sdlog2/sdlog2_format.h
@@ -85,10 +85,10 @@ struct log_format_s {
 
 #define LOG_FORMAT(_name, _format, _labels) { \
 		.type = LOG_##_name##_MSG, \
-		.length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \
-		.name = #_name, \
-		.format = _format, \
-		.labels = _labels \
+			.length = sizeof(struct log_##_name##_s) + LOG_PACKET_HEADER_LEN, \
+				  .name = #_name, \
+					  .format = _format, \
+						    .labels = _labels \
 	}
 
 #define LOG_FORMAT_MSG	  0x80
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 9f709e4591..90093a407c 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -48,12 +48,6 @@
 /* define message formats */
 
 #pragma pack(push, 1)
-/* --- TIME - TIME STAMP --- */
-#define LOG_TIME_MSG 1
-struct log_TIME_s {
-	uint64_t t;
-};
-
 /* --- ATT - ATTITUDE --- */
 #define LOG_ATT_MSG 2
 struct log_ATT_s {
@@ -253,18 +247,31 @@ struct log_GVSP_s {
 	float vz;
 };
 
+/* --- TIME - TIME STAMP --- */
+#define LOG_TIME_MSG 129
+struct log_TIME_s {
+	uint64_t t;
+};
+
 /* --- VER - VERSION --- */
-#define LOG_VER_MSG 127
+#define LOG_VER_MSG 130
 struct log_VER_s {
 	char arch[16];
 	char fw_git[64];
 };
 
+/* --- PARM - PARAMETER --- */
+#define LOG_PARM_MSG 131
+struct log_PARM_s {
+	char name[16];
+	float value;
+};
+
 #pragma pack(pop)
 
 /* construct list of all message formats */
 static const struct log_format_s log_formats[] = {
-	LOG_FORMAT(TIME, "Q", "StartTime"),
+	/* business-level messages, ID < 0x80 */
 	LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"),
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
@@ -283,7 +290,11 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,N,Ver,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 	LOG_FORMAT(GVSP, "fff", "VX,VY,VZ"),
+	/* system-level messages, ID >= 0x80 */
+	// FMT: don't write format of format message, it's useless
+	LOG_FORMAT(TIME, "Q", "StartTime"),
 	LOG_FORMAT(VER, "NZ", "Arch,FwGit"),
+	LOG_FORMAT(PARM, "Nf", "Name,Value"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);

From bb8a2c3631a518f822d1e7e3d40c28a281901a3f Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 23 Oct 2013 18:57:44 +0200
Subject: [PATCH 35/47] sdlog2_dump.py: C strings parsing fixed

---
 Tools/sdlog2_dump.py | 25 ++++++++++++-------------
 1 file changed, 12 insertions(+), 13 deletions(-)

diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2_dump.py
index 7fefc5908f..5b1e55e22a 100644
--- a/Tools/sdlog2_dump.py
+++ b/Tools/sdlog2_dump.py
@@ -25,8 +25,12 @@ import struct, sys
 
 if sys.hexversion >= 0x030000F0:
     runningPython3 = True
+    def _parseCString(cstr):
+        return str(cstr, 'ascii').split('\0')[0]
 else:
     runningPython3 = False
+    def _parseCString(cstr):
+        return str(cstr).split('\0')[0]
 
 class SDLog2Parser:
     BLOCK_SIZE = 8192
@@ -175,9 +179,9 @@ class SDLog2Parser:
                 self.__csv_columns.append(full_label)
                 self.__csv_data[full_label] = None
         if self.__file != None:
-                print(self.__csv_delim.join(self.__csv_columns), file=self.__file)
+            print(self.__csv_delim.join(self.__csv_columns), file=self.__file)
         else:
-                print(self.__csv_delim.join(self.__csv_columns))
+            print(self.__csv_delim.join(self.__csv_columns))
 
     def __printCSVRow(self):
         s = []
@@ -190,9 +194,9 @@ class SDLog2Parser:
             s.append(v)
 
         if self.__file != None:
-                print(self.__csv_delim.join(s), file=self.__file)
+            print(self.__csv_delim.join(s), file=self.__file)
         else:
-                print(self.__csv_delim.join(s))
+            print(self.__csv_delim.join(s))
 
     def __parseMsgDescr(self):
         if runningPython3:
@@ -202,14 +206,9 @@ class SDLog2Parser:
         msg_type = data[0]
         if msg_type != self.MSG_TYPE_FORMAT:
             msg_length = data[1]
-            if runningPython3:
-                msg_name = str(data[2], 'ascii').strip("\0")
-                msg_format = str(data[3], 'ascii').strip("\0")
-                msg_labels = str(data[4], 'ascii').strip("\0").split(",")
-            else:
-                msg_name = str(data[2]).strip("\0")
-                msg_format = str(data[3]).strip("\0")
-                msg_labels = str(data[4]).strip("\0").split(",")
+            msg_name = _parseCString(data[2])
+            msg_format = _parseCString(data[3])
+            msg_labels = _parseCString(data[4]).split(",")
             # Convert msg_format to struct.unpack format string
             msg_struct = ""
             msg_mults = []
@@ -243,7 +242,7 @@ class SDLog2Parser:
                 data = list(struct.unpack(msg_struct, str(self.__buffer[self.__ptr+self.MSG_HEADER_LEN:self.__ptr+msg_length])))
             for i in range(len(data)):
                 if type(data[i]) is str:
-                    data[i] = data[i].strip("\0")
+                    data[i] = _parseCString(data[i])
                 m = msg_mults[i]
                 if m != None:
                     data[i] = data[i] * m

From 28845c4846e4f225e686f6643b5d2e0851392b2b Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sat, 19 Oct 2013 09:45:34 +0200
Subject: [PATCH 36/47] Rascal (AERT) hil startup script

---
 ...1000_rc_fw.hil => 1000_rc_fw_easystar.hil} |   0
 .../init.d/1004_rc_fw_Rascal110.hil           | 103 ++++++++++++++++++
 ROMFS/px4fmu_common/init.d/rcS                |   8 +-
 3 files changed, 110 insertions(+), 1 deletion(-)
 rename ROMFS/px4fmu_common/init.d/{1000_rc_fw.hil => 1000_rc_fw_easystar.hil} (100%)
 create mode 100644 ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil

diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
similarity index 100%
rename from ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
rename to ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
diff --git a/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
new file mode 100644
index 0000000000..8c5e4b6a8f
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1004_rc_fw_Rascal110.hil
@@ -0,0 +1,103 @@
+#!nsh
+#
+# USB HIL start
+#
+
+echo "[HIL] HILStar starting.."
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	
+	param set FW_P_D 0
+	param set FW_P_I 0
+	param set FW_P_IMAX 15
+	param set FW_P_LIM_MAX 50
+	param set FW_P_LIM_MIN -50
+	param set FW_P_P 60
+	param set FW_P_RMAX_NEG 0
+	param set FW_P_RMAX_POS 0
+	param set FW_P_ROLLFF 1.1
+	param set FW_R_D 0
+	param set FW_R_I 5
+	param set FW_R_IMAX 20
+	param set FW_R_P 100
+	param set FW_R_RMAX 100
+	param set FW_THR_CRUISE 0.65
+	param set FW_THR_MAX 1
+	param set FW_THR_MIN 0
+	param set FW_T_SINK_MAX 5.0
+	param set FW_T_SINK_MIN 4.0
+	param set FW_Y_ROLLFF 1.1
+	param set FW_L1_PERIOD 16
+	param set RC_SCALE_ROLL 1.0
+	param set RC_SCALE_PITCH 1.0
+
+	param set SYS_AUTOCONFIG 0
+	param save
+fi
+
+# Allow USB some time to come up
+sleep 1
+# Tell MAVLink that this link is "fast"
+mavlink start -b 230400 -d /dev/ttyACM0
+
+# Create a fake HIL /dev/pwm_output interface
+hil mode_pwm
+
+#
+# Force some key parameters to sane values
+# MAV_TYPE     1 = fixed wing, 2 = quadrotor, 13 = hexarotor
+#              see https://pixhawk.ethz.ch/mavlink/
+#
+param set MAV_TYPE 1
+
+#
+# Start the commander (depends on orb, mavlink)
+#
+commander start
+
+#
+# Check if we got an IO
+#
+if px4io start
+then
+	echo "IO started"
+else
+	fmu mode_serial
+	echo "FMU started"
+fi
+
+#
+# Start the sensors (depends on orb, px4io)
+#
+sh /etc/init.d/rc.sensors
+
+#
+# Start the attitude estimator (depends on orb)
+#
+att_pos_estimator_ekf start
+ 
+#
+# Load mixer and start controllers (depends on px4io)
+#
+set MODE autostart
+mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
+if [ -f /fs/microsd/etc/mixers/FMU_AERT.mix ]
+then
+	echo "Using /fs/microsd/etc/mixers/FMU_AERT.mix"
+	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_AERT.mix
+else
+	echo "Using /etc/mixers/FMU_AERT.mix"
+	mixer load /dev/pwm_output /etc/mixers/FMU_AERT.mix
+fi
+
+
+fw_pos_control_l1 start
+fw_att_control start
+
+echo "[HIL] setup done, running"
+
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 424787d54a..94c01419e3 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -108,7 +108,7 @@ then
 
 	if param compare SYS_AUTOSTART 1000
 	then
-		sh /etc/init.d/1000_rc_fw.hil		
+		sh /etc/init.d/1000_rc_fw_easystar.hil		
 		set MODE custom
 	fi
 	
@@ -129,6 +129,12 @@ then
 		sh /etc/init.d/1003_rc_quad_+.hil
 		set MODE custom
 	fi
+
+	if param compare SYS_AUTOSTART 1004
+	then
+		sh /etc/init.d/1004_rc_fw_Rascal110.hil
+		set MODE custom
+	fi
 	
 	if [ $MODE != custom ]
 	then

From 2cd9ad97eaf20c43a4c519413b258712d844f1d1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 24 Oct 2013 09:26:03 +0200
Subject: [PATCH 37/47] Removed unnecessary return statements

---
 src/drivers/ets_airspeed/ets_airspeed.cpp   | 3 ---
 src/drivers/meas_airspeed/meas_airspeed.cpp | 3 ---
 2 files changed, 6 deletions(-)

diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index 084671ae20..9d8ad084e5 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -132,11 +132,8 @@ ETSAirspeed::measure()
 	if (OK != ret) {
 		perf_count(_comms_errors);
 		log("i2c::transfer returned %d", ret);
-		return ret;
 	}
 
-	ret = OK;
-
 	return ret;
 }
 
diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index ee45d46acb..b3003fc1b0 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -138,11 +138,8 @@ MEASAirspeed::measure()
 
 	if (OK != ret) {
 		perf_count(_comms_errors);
-		return ret;
 	}
 
-	ret = OK;
-
 	return ret;
 }
 

From cc324f2624cc3a4347d736fd5634672d5a2716e9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 24 Oct 2013 09:28:14 +0200
Subject: [PATCH 38/47] Ignoring docs output

---
 Tools/px4params/.gitignore | 2 ++
 1 file changed, 2 insertions(+)
 create mode 100644 Tools/px4params/.gitignore

diff --git a/Tools/px4params/.gitignore b/Tools/px4params/.gitignore
new file mode 100644
index 0000000000..73cf39575d
--- /dev/null
+++ b/Tools/px4params/.gitignore
@@ -0,0 +1,2 @@
+parameters.wiki
+parameters.xml
\ No newline at end of file

From 1cb73687f7acd4ae3263c40940afe057b1b7d368 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 20 Oct 2013 21:51:45 +0200
Subject: [PATCH 39/47] added parameter for maximal roll angle

---
 src/lib/ecl/l1/ecl_l1_pos_controller.cpp              |  2 +-
 src/lib/ecl/l1/ecl_l1_pos_controller.h                | 11 +++++++++++
 .../fw_pos_control_l1/fw_pos_control_l1_main.cpp      |  5 +++++
 .../fw_pos_control_l1/fw_pos_control_l1_params.c      |  3 +++
 4 files changed, 20 insertions(+), 1 deletion(-)

diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index daf136d495..196ded26ca 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -43,7 +43,7 @@
 float ECL_L1_Pos_Controller::nav_roll()
 {
 	float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
-	ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f);
+	ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad);
 	return ret;
 }
 
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h
index 5a17346cb1..7a3c42a925 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.h
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h
@@ -222,6 +222,15 @@ public:
 		_K_L1 = 4.0f * _L1_damping * _L1_damping;
 	}
 
+
+	/**
+	 * Set the maximum roll angle output in radians
+	 *
+	 */
+	void set_l1_roll_limit(float roll_lim_rad) {
+		_roll_lim_rad = roll_lim_rad;
+	}
+
 private:
 
 	float _lateral_accel;		///< Lateral acceleration setpoint in m/s^2
@@ -238,6 +247,8 @@ private:
 	float _K_L1;			///< L1 control gain for _L1_damping
 	float _heading_omega;		///< Normalized frequency
 
+	float _roll_lim_rad;  ///<maximum roll angle
+
 	/**
 	 * Convert a 2D vector from WGS84 to planar coordinates.
 	 *
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index cd4a0d58ef..f09e44b395 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -192,6 +192,7 @@ private:
 
 		float pitch_limit_min;
 		float pitch_limit_max;
+		float roll_limit;
 		float throttle_min;
 		float throttle_max;
 		float throttle_cruise;
@@ -223,6 +224,7 @@ private:
 
 		param_t pitch_limit_min;
 		param_t pitch_limit_max;
+		param_t roll_limit;
 		param_t throttle_min;
 		param_t throttle_max;
 		param_t throttle_cruise;
@@ -339,6 +341,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 	_parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN");
 	_parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX");
+	_parameter_handles.roll_limit = param_find("FW_R_LIM");
 	_parameter_handles.throttle_min = param_find("FW_THR_MIN");
 	_parameter_handles.throttle_max = param_find("FW_THR_MAX");
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
@@ -400,6 +403,7 @@ FixedwingPositionControl::parameters_update()
 
 	param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min));
 	param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max));
+	param_get(_parameter_handles.roll_limit, &(_parameters.roll_limit));
 	param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
 	param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
 	param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
@@ -419,6 +423,7 @@ FixedwingPositionControl::parameters_update()
 
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
+	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
 
 	_tecs.set_time_const(_parameters.time_const);
 	_tecs.set_min_sink_rate(_parameters.min_sink_rate);
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index d210ec7126..bf4b3b32a9 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -67,6 +67,9 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
 PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
 
 
+PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
+
+
 PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 
 

From db1fe9f0fa239c47f88148ec344c28ecdca21198 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 29 Sep 2013 19:00:12 +0200
Subject: [PATCH 40/47] adding skywalker x5 startup script

---
 ROMFS/px4fmu_common/init.d/32_skywalker_x5 | 86 ++++++++++++++++++++++
 ROMFS/px4fmu_common/init.d/rcS             |  6 ++
 2 files changed, 92 insertions(+)
 create mode 100644 ROMFS/px4fmu_common/init.d/32_skywalker_x5

diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5
new file mode 100644
index 0000000000..432cbb14e2
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/32_skywalker_x5
@@ -0,0 +1,86 @@
+#!nsh
+
+echo "[init] PX4FMU v1, v2 with or without IO on Skywalker X5"
+
+#
+# Load default params for this platform
+#
+if param compare SYS_AUTOCONFIG 1
+then
+	# Set all params here, then disable autoconfig
+	# TODO
+	
+	param set SYS_AUTOCONFIG 0
+	param save
+fi
+ 
+#
+# Force some key parameters to sane values
+# MAV_TYPE     1 = fixed wing
+#
+param set MAV_TYPE 1
+
+set EXIT_ON_END no
+
+#
+# Start and configure PX4IO or FMU interface
+#
+if px4io detect
+then
+	# Start MAVLink (depends on orb)
+	mavlink start
+
+	commander start
+
+	sh /etc/init.d/rc.io
+	# Limit to 100 Hz updates and (implicit) 50 Hz PWM
+	px4io limit 100
+else
+	# Start MAVLink (on UART1 / ttyS0)
+	mavlink start -d /dev/ttyS0
+
+	commander start
+
+	fmu mode_pwm
+	param set BAT_V_SCALING 0.004593
+	set EXIT_ON_END yes
+fi
+ 
+#
+# Start the sensors and test them.
+#
+sh /etc/init.d/rc.sensors
+
+#
+# Start logging (depends on sensors)
+#
+sh /etc/init.d/rc.logging
+
+#
+# Start GPS interface (depends on orb)
+#
+gps start
+ 
+#
+# Start the attitude and position estimator
+#
+att_pos_estimator_ekf start
+
+#
+# Load mixer and start controllers (depends on px4io)
+#
+if [ -f /fs/microsd/etc/mixers/FMU_delta.mix ]
+then
+	echo "Using FMU_delta mixer from sd card"
+	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_delta.mix
+else
+	echo "Using standard FMU_delta mixer"
+	mixer load /dev/pwm_output /etc/mixers/FMU_delta.mix
+fi
+fw_att_control start
+fw_pos_control_l1 start
+
+if [ $EXIT_ON_END == yes ]
+then
+	exit
+fi
diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 94c01419e3..9d0800eb17 100755
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -284,6 +284,12 @@ then
 		sh /etc/init.d/31_io_phantom
 		set MODE custom
 	fi
+	
+	if param compare SYS_AUTOSTART 32
+	then
+		sh /etc/init.d/32_skywalker_x5
+		set MODE custom
+	fi
 
 	if param compare SYS_AUTOSTART 40
 	then

From 8193382ec245ff82ffb30e2d9038c253bc93a099 Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Thu, 24 Oct 2013 17:25:14 +0200
Subject: [PATCH 41/47] change default mixer for skywalker x5

---
 ROMFS/px4fmu_common/init.d/32_skywalker_x5 | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/32_skywalker_x5 b/ROMFS/px4fmu_common/init.d/32_skywalker_x5
index 432cbb14e2..cd7677112d 100644
--- a/ROMFS/px4fmu_common/init.d/32_skywalker_x5
+++ b/ROMFS/px4fmu_common/init.d/32_skywalker_x5
@@ -69,13 +69,13 @@ att_pos_estimator_ekf start
 #
 # Load mixer and start controllers (depends on px4io)
 #
-if [ -f /fs/microsd/etc/mixers/FMU_delta.mix ]
+if [ -f /fs/microsd/etc/mixers/FMU_Q.mix ]
 then
-	echo "Using FMU_delta mixer from sd card"
-	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_delta.mix
+	echo "Using /fs/microsd/etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /fs/microsd/etc/mixers/FMU_Q.mix
 else
-	echo "Using standard FMU_delta mixer"
-	mixer load /dev/pwm_output /etc/mixers/FMU_delta.mix
+	echo "Using /etc/mixers/FMU_Q.mix"
+	mixer load /dev/pwm_output /etc/mixers/FMU_Q.mix
 fi
 fw_att_control start
 fw_pos_control_l1 start

From 4b63c5488582241c7e3af45dce74c112dbfa60bd Mon Sep 17 00:00:00 2001
From: Thomas Gubler <thomasgubler@gmail.com>
Date: Sun, 27 Oct 2013 14:48:12 +0100
Subject: [PATCH 42/47] l1: fix constrain of sine_eta1

---
 src/lib/ecl/l1/ecl_l1_pos_controller.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index 196ded26ca..27d76f959c 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -190,7 +190,7 @@ void ECL_L1_Pos_Controller::navigate_waypoints(const math::Vector2f &vector_A, c
 		float xtrackErr = vector_A_to_airplane % vector_AB;
 		float sine_eta1 = xtrackErr / math::max(_L1_distance , 0.1f);
 		/* limit output to 45 degrees */
-		sine_eta1 = math::constrain(sine_eta1, -M_PI_F / 4.0f, +M_PI_F / 4.0f);
+		sine_eta1 = math::constrain(sine_eta1, -0.7071f, 0.7071f); //sin(pi/4) = 0.7071
 		float eta1 = asinf(sine_eta1);
 		eta = eta1 + eta2;
 		/* bearing from current position to L1 point */

From 9064f8bf09dd91388b9fd3e66568d086bf1be69b Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 28 Oct 2013 15:49:26 +1100
Subject: [PATCH 43/47] px4io: fixed the io_reg_{set,get} errors

this fixes the PX4IO state machine to avoid the io errors we were
seeing. There are still some open questions with this code, but it now
seems to give zero errors, which is an improvement!
---
 src/modules/px4iofirmware/i2c.c    |  6 ------
 src/modules/px4iofirmware/px4io.c  | 10 ++--------
 src/modules/px4iofirmware/serial.c | 20 +++-----------------
 3 files changed, 5 insertions(+), 31 deletions(-)

diff --git a/src/modules/px4iofirmware/i2c.c b/src/modules/px4iofirmware/i2c.c
index 10aeb5c9fa..79b6546b39 100644
--- a/src/modules/px4iofirmware/i2c.c
+++ b/src/modules/px4iofirmware/i2c.c
@@ -149,12 +149,6 @@ interface_init(void)
 #endif
 }
 
-void
-interface_tick()
-{
-}
-
-
 /*
   reset the I2C bus
   used to recover from lockups
diff --git a/src/modules/px4iofirmware/px4io.c b/src/modules/px4iofirmware/px4io.c
index e70b3fe880..e28106971a 100644
--- a/src/modules/px4iofirmware/px4io.c
+++ b/src/modules/px4iofirmware/px4io.c
@@ -159,9 +159,6 @@ user_start(int argc, char *argv[])
 	/* start the FMU interface */
 	interface_init();
 
-	/* add a performance counter for the interface */
-	perf_counter_t interface_perf = perf_alloc(PC_ELAPSED, "interface");
-
 	/* add a performance counter for mixing */
 	perf_counter_t mixer_perf = perf_alloc(PC_ELAPSED, "mix");
 
@@ -203,11 +200,6 @@ user_start(int argc, char *argv[])
 		/* track the rate at which the loop is running */
 		perf_count(loop_perf);
 
-		/* kick the interface */
-		perf_begin(interface_perf);
-		interface_tick();
-		perf_end(interface_perf);
-
 		/* kick the mixer */
 		perf_begin(mixer_perf);
 		mixer_tick();
@@ -218,6 +210,7 @@ user_start(int argc, char *argv[])
 		controls_tick();
 		perf_end(controls_perf);
 
+#if 0
 		/* check for debug activity */
 		show_debug_messages();
 
@@ -234,6 +227,7 @@ user_start(int argc, char *argv[])
 				  (unsigned)minfo.mxordblk);
 			last_debug_time = hrt_absolute_time();
 		}
+#endif
 	}
 }
 
diff --git a/src/modules/px4iofirmware/serial.c b/src/modules/px4iofirmware/serial.c
index 94d7407dff..e9adc8cd69 100644
--- a/src/modules/px4iofirmware/serial.c
+++ b/src/modules/px4iofirmware/serial.c
@@ -74,9 +74,6 @@ static DMA_HANDLE	rx_dma;
 static int		serial_interrupt(int irq, void *context);
 static void		dma_reset(void);
 
-/* if we spend this many ticks idle, reset the DMA */
-static unsigned		idle_ticks;
-
 static struct IOPacket	dma_packet;
 
 /* serial register accessors */
@@ -150,16 +147,6 @@ interface_init(void)
 	debug("serial init");
 }
 
-void
-interface_tick()
-{
-	/* XXX look for stuck/damaged DMA and reset? */
-	if (idle_ticks++ > 100) {
-		dma_reset();
-		idle_ticks = 0;
-	}
-}
-
 static void
 rx_handle_packet(void)
 {
@@ -230,9 +217,6 @@ rx_dma_callback(DMA_HANDLE handle, uint8_t status, void *arg)
 	/* disable UART DMA */
 	rCR3 &= ~(USART_CR3_DMAT | USART_CR3_DMAR);
 
-	/* reset the idle counter */
-	idle_ticks = 0;
-
 	/* handle the received packet */
 	rx_handle_packet();
 
@@ -308,6 +292,7 @@ serial_interrupt(int irq, void *context)
 
 			/* it was too short - possibly truncated */
 			perf_count(pc_badidle);
+			dma_reset();
 			return 0;
 		}
 
@@ -343,7 +328,8 @@ dma_reset(void)
 		sizeof(dma_packet),
 		DMA_CCR_MINC		|
 		DMA_CCR_PSIZE_8BITS	|
-		DMA_CCR_MSIZE_8BITS);
+		DMA_CCR_MSIZE_8BITS     |
+		DMA_CCR_PRIVERYHI);
 
 	/* start receive DMA ready for the next packet */
 	stm32_dmastart(rx_dma, rx_dma_callback, NULL, false);

From 75a0c18a9e56ac64e043cce00223ea8a627d24a5 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 28 Oct 2013 15:49:57 +1100
Subject: [PATCH 44/47] px4io: FMU half of px4io error fixes

---
 src/drivers/px4io/px4io_serial.cpp | 28 +++++++++++++++++-----------
 1 file changed, 17 insertions(+), 11 deletions(-)

diff --git a/src/drivers/px4io/px4io_serial.cpp b/src/drivers/px4io/px4io_serial.cpp
index 236cb918dc..43318ca849 100644
--- a/src/drivers/px4io/px4io_serial.cpp
+++ b/src/drivers/px4io/px4io_serial.cpp
@@ -48,6 +48,7 @@
 #include <time.h>
 #include <errno.h>
 #include <string.h>
+#include <stdio.h>
 
 #include <arch/board/board.h>
 
@@ -262,7 +263,8 @@ PX4IO_serial::init()
 	up_enable_irq(PX4IO_SERIAL_VECTOR);
 
 	/* enable UART in DMA mode, enable error and line idle interrupts */
-	/* rCR3 = USART_CR3_EIE;*/
+	rCR3 = USART_CR3_EIE;
+
 	rCR1 = USART_CR1_RE | USART_CR1_TE | USART_CR1_UE | USART_CR1_IDLEIE;
 
 	/* create semaphores */
@@ -497,22 +499,20 @@ PX4IO_serial::_wait_complete()
 		DMA_SCR_PBURST_SINGLE	|
 		DMA_SCR_MBURST_SINGLE);
 	stm32_dmastart(_tx_dma, nullptr, nullptr, false);
+	//rCR1 &= ~USART_CR1_TE;
+	//rCR1 |= USART_CR1_TE;
 	rCR3 |= USART_CR3_DMAT;
 
 	perf_end(_pc_dmasetup);
 
-	/* compute the deadline for a 5ms timeout */
+	/* compute the deadline for a 10ms timeout */
 	struct timespec abstime;
 	clock_gettime(CLOCK_REALTIME, &abstime);
-#if 0
-	abstime.tv_sec++;		/* long timeout for testing */
-#else
-	abstime.tv_nsec += 10000000;	/* 0ms timeout */
-	if (abstime.tv_nsec > 1000000000) {
+	abstime.tv_nsec += 10*1000*1000;
+	if (abstime.tv_nsec >= 1000*1000*1000) {
 		abstime.tv_sec++;
-		abstime.tv_nsec -= 1000000000;
+		abstime.tv_nsec -= 1000*1000*1000;
 	}
-#endif
 
 	/* wait for the transaction to complete - 64 bytes @ 1.5Mbps ~426µs */
 	int ret;
@@ -619,8 +619,8 @@ PX4IO_serial::_do_interrupt()
 		 */
 		if (_rx_dma_status == _dma_status_waiting) {
 			_abort_dma();
-			perf_count(_pc_uerrs);
 
+			perf_count(_pc_uerrs);
 			/* complete DMA as though in error */
 			_do_rx_dma_callback(DMA_STATUS_TEIF);
 
@@ -642,6 +642,12 @@ PX4IO_serial::_do_interrupt()
 			unsigned length = sizeof(_dma_buffer) - stm32_dmaresidual(_rx_dma);
 			if ((length < 1) || (length < PKT_SIZE(_dma_buffer))) {
 				perf_count(_pc_badidle);
+
+				/* stop the receive DMA */
+				stm32_dmastop(_rx_dma);
+
+				/* complete the short reception */
+				_do_rx_dma_callback(DMA_STATUS_TEIF);
 				return;
 			}
 
@@ -670,4 +676,4 @@ PX4IO_serial::_abort_dma()
 	stm32_dmastop(_rx_dma);
 }
 
-#endif /* PX4IO_SERIAL_BASE */
\ No newline at end of file
+#endif /* PX4IO_SERIAL_BASE */

From 52ee477137a293d40fc552f4100c52d19b142fc1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <tridge@samba.org>
Date: Mon, 28 Oct 2013 15:50:33 +1100
Subject: [PATCH 45/47] lsm303d: try to reset the lsm303d if it goes bad in
 flight

this is based on earlier work by Julian Oes
---
 src/drivers/lsm303d/lsm303d.cpp | 32 +++++++++++++++++++++++++++++---
 1 file changed, 29 insertions(+), 3 deletions(-)

diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp
index 8bed8a8df0..60601e22c8 100644
--- a/src/drivers/lsm303d/lsm303d.cpp
+++ b/src/drivers/lsm303d/lsm303d.cpp
@@ -241,11 +241,18 @@ private:
 
 	perf_counter_t		_accel_sample_perf;
 	perf_counter_t		_mag_sample_perf;
+	perf_counter_t		_reg7_resets;
+	perf_counter_t		_reg1_resets;
 
 	math::LowPassFilter2p	_accel_filter_x;
 	math::LowPassFilter2p	_accel_filter_y;
 	math::LowPassFilter2p	_accel_filter_z;
 
+	// expceted values of reg1 and reg7 to catch in-flight
+	// brownouts of the sensor
+	uint8_t			_reg7_expected;
+	uint8_t			_reg1_expected;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -434,9 +441,13 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device) :
 	_mag_read(0),
 	_accel_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_accel_read")),
 	_mag_sample_perf(perf_alloc(PC_ELAPSED, "lsm303d_mag_read")),
+	_reg1_resets(perf_alloc(PC_COUNT, "lsm303d_reg1_resets")),
+	_reg7_resets(perf_alloc(PC_COUNT, "lsm303d_reg7_resets")),
 	_accel_filter_x(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
 	_accel_filter_y(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
-	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ)
+	_accel_filter_z(LSM303D_ACCEL_DEFAULT_RATE, LSM303D_ACCEL_DEFAULT_DRIVER_FILTER_FREQ),
+	_reg1_expected(0),
+	_reg7_expected(0)
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -526,10 +537,12 @@ void
 LSM303D::reset()
 {
 	/* enable accel*/
-	write_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE);
+	_reg1_expected = REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A;
+	write_reg(ADDR_CTRL_REG1, _reg1_expected);
 
 	/* enable mag */
-	write_reg(ADDR_CTRL_REG7, REG7_CONT_MODE_M);
+	_reg7_expected = REG7_CONT_MODE_M;
+	write_reg(ADDR_CTRL_REG7, _reg7_expected);
 	write_reg(ADDR_CTRL_REG5, REG5_RES_HIGH_M);
 
 	accel_set_range(LSM303D_ACCEL_DEFAULT_RANGE_G);
@@ -1133,6 +1146,7 @@ LSM303D::accel_set_samplerate(unsigned frequency)
 	}
 
 	modify_reg(ADDR_CTRL_REG1, clearbits, setbits);
+	_reg1_expected = (_reg1_expected & ~clearbits) | setbits;
 
 	return OK;
 }
@@ -1210,6 +1224,12 @@ LSM303D::mag_measure_trampoline(void *arg)
 void
 LSM303D::measure()
 {
+	if (read_reg(ADDR_CTRL_REG1) != _reg1_expected) {
+		perf_count(_reg1_resets);
+		reset();
+		return;
+	}
+
 	/* status register and data as read back from the device */
 
 #pragma pack(push, 1)
@@ -1282,6 +1302,12 @@ LSM303D::measure()
 void
 LSM303D::mag_measure()
 {
+	if (read_reg(ADDR_CTRL_REG7) != _reg7_expected) {
+		perf_count(_reg7_resets);
+		reset();
+		return;
+	}
+
 	/* status register and data as read back from the device */
 #pragma pack(push, 1)
 	struct {

From 1336d625a83ba3f1afc207b64ec248dd1e15848a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 28 Oct 2013 14:47:37 +0100
Subject: [PATCH 46/47] Hotfix: Announcing important messages via audio

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

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 9814a7bccf..44a1442968 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -379,7 +379,7 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			}
 
 			// TODO remove debug code
-			//mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_main_mode);
+			//mavlink_log_critical(mavlink_fd, "#audio: command setmode: %d %d", base_mode, custom_main_mode);
 			/* set arming state */
 			arming_res = TRANSITION_NOT_CHANGED;
 
@@ -496,11 +496,11 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				}
 
 				if (arming_res == TRANSITION_CHANGED) {
-					mavlink_log_info(mavlink_fd, "[cmd] ARMED by component arm cmd");
+					mavlink_log_critical(mavlink_fd, "#audio: ARMED by component arm cmd");
 					result = VEHICLE_CMD_RESULT_ACCEPTED;
 
 				} else {
-					mavlink_log_info(mavlink_fd, "[cmd] REJECTING component arm cmd");
+					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
 					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
 				}
 			}
@@ -521,13 +521,13 @@ void handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		tune_negative();
 
 		if (result == VEHICLE_CMD_RESULT_DENIED) {
-			mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command);
+			mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd->command);
 
 		} else if (result == VEHICLE_CMD_RESULT_FAILED) {
-			mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command);
+			mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd->command);
 
 		} else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) {
-			mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command);
+			mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
 
 		}
 	}
@@ -874,10 +874,10 @@ int commander_thread_main(int argc, char *argv[])
 				status_changed = true;
 
 				if (status.condition_landed) {
-					mavlink_log_critical(mavlink_fd, "[cmd] LANDED");
+					mavlink_log_critical(mavlink_fd, "#audio: LANDED");
 
 				} else {
-					mavlink_log_critical(mavlink_fd, "[cmd] IN AIR");
+					mavlink_log_critical(mavlink_fd, "#audio: IN AIR");
 				}
 			}
 		}
@@ -955,7 +955,7 @@ int commander_thread_main(int argc, char *argv[])
 			//TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS
 			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
 				low_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
+				mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
 				status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
 				status_changed = true;
 				battery_tune_played = false;
@@ -967,7 +967,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
 				critical_battery_voltage_actions_done = true;
-				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
+				mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
 				status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 				battery_tune_played = false;
 
@@ -1069,12 +1069,12 @@ int commander_thread_main(int argc, char *argv[])
 				/* handle the case where RC signal was regained */
 				if (!status.rc_signal_found_once) {
 					status.rc_signal_found_once = true;
-					mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time");
+					mavlink_log_critical(mavlink_fd, "#audio: detected RC signal first time");
 					status_changed = true;
 
 				} else {
 					if (status.rc_signal_lost) {
-						mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained");
+						mavlink_log_critical(mavlink_fd, "#audio: RC signal regained");
 						status_changed = true;
 					}
 				}
@@ -1143,7 +1143,7 @@ int commander_thread_main(int argc, char *argv[])
 
 				} else if (res == TRANSITION_DENIED) {
 					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+					mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
 				}
 
 				/* fill current_status according to mode switches */
@@ -1159,12 +1159,12 @@ int commander_thread_main(int argc, char *argv[])
 				} else if (res == TRANSITION_DENIED) {
 					/* DENIED here indicates bug in the commander */
 					warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
-					mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
+					mavlink_log_critical(mavlink_fd, "#audio: ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch);
 				}
 
 			} else {
 				if (!status.rc_signal_lost) {
-					mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: RC SIGNAL LOST");
+					mavlink_log_critical(mavlink_fd, "#audio: CRITICAL: RC SIGNAL LOST");
 					status.rc_signal_lost = true;
 					status_changed = true;
 				}
@@ -1189,7 +1189,7 @@ int commander_thread_main(int argc, char *argv[])
 		if (res == TRANSITION_DENIED) {
 			/* DENIED here indicates bug in the commander */
 			warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
-			mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
+			mavlink_log_critical(mavlink_fd, "#audio: ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state);
 		}
 
 		/* check which state machines for changes, clear "changed" flag */
@@ -1506,7 +1506,7 @@ print_reject_mode(const char *msg)
 	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
 		last_print_mode_reject_time = t;
 		char s[80];
-		sprintf(s, "[cmd] WARNING: reject %s", msg);
+		sprintf(s, "#audio: warning: reject %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
 		tune_negative();
 	}
@@ -1520,7 +1520,7 @@ print_reject_arm(const char *msg)
 	if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
 		last_print_mode_reject_time = t;
 		char s[80];
-		sprintf(s, "[cmd] %s", msg);
+		sprintf(s, "#audio: %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
 		tune_negative();
 	}
@@ -1617,14 +1617,14 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 
 			if (res == TRANSITION_CHANGED) {
 				if (control_mode->flag_control_position_enabled) {
-					mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: POS HOLD");
+					mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: POS HOLD");
 
 				} else {
 					if (status->condition_landed) {
-						mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD (LAND)");
+						mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD (LAND)");
 
 					} else {
-						mavlink_log_critical(mavlink_fd, "[cmd] FAILSAFE: ALT HOLD");
+						mavlink_log_critical(mavlink_fd, "#audio: FAILSAFE: ALT HOLD");
 					}
 				}
 			}
@@ -1660,22 +1660,22 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
-		mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd.command);
+		mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
 		tune_negative();
 		break;
 
 	case VEHICLE_CMD_RESULT_FAILED:
-		mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd.command);
+		mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
 		tune_negative();
 		break;
 
 	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
-		mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd.command);
+		mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
 		tune_negative();
 		break;
 
 	case VEHICLE_CMD_RESULT_UNSUPPORTED:
-		mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd.command);
+		mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
 		tune_negative();
 		break;
 
@@ -1814,14 +1814,14 @@ void *commander_low_prio_loop(void *arg)
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
-						mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+						mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR");
 
 						/* convenience as many parts of NuttX use negative errno */
 						if (ret < 0)
 							ret = -ret;
 
 						if (ret < 1000)
-							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
+							mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
 
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
@@ -1834,14 +1834,14 @@ void *commander_low_prio_loop(void *arg)
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
-						mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+						mavlink_log_critical(mavlink_fd, "#audio: parameters save error");
 
 						/* convenience as many parts of NuttX use negative errno */
 						if (ret < 0)
 							ret = -ret;
 
 						if (ret < 1000)
-							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
+							mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
 
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}

From 0fa03e65ab3ab0e173e487b3e5f5321780f3afff Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 28 Oct 2013 15:21:50 +0100
Subject: [PATCH 47/47] Cleanup of Doxygen tags

---
 src/drivers/drv_accel.h      | 4 +++-
 src/drivers/drv_airspeed.h   | 5 ++++-
 src/drivers/drv_baro.h       | 4 +++-
 src/drivers/drv_gps.h        | 4 +++-
 src/drivers/drv_gyro.h       | 4 +++-
 src/drivers/drv_rc_input.h   | 3 ++-
 src/drivers/drv_sensor.h     | 4 +++-
 src/drivers/drv_tone_alarm.h | 4 +++-
 8 files changed, 24 insertions(+), 8 deletions(-)

diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index 8a4f684284..7d93ed9383 100644
--- a/src/drivers/drv_accel.h
+++ b/src/drivers/drv_accel.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Accelerometer driver interface.
+ * @file drv_accel.h
+ *
+ * Accelerometer driver interface.
  */
 
 #ifndef _DRV_ACCEL_H
diff --git a/src/drivers/drv_airspeed.h b/src/drivers/drv_airspeed.h
index 7bb9ee2af1..78f31495d9 100644
--- a/src/drivers/drv_airspeed.h
+++ b/src/drivers/drv_airspeed.h
@@ -32,7 +32,10 @@
  ****************************************************************************/
 
 /**
- * @file Airspeed driver interface.
+ * @file drv_airspeed.h
+ *
+ * Airspeed driver interface.
+ * 
  * @author Simon Wilks
  */
 
diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h
index aa251889f7..e2f0137ae2 100644
--- a/src/drivers/drv_baro.h
+++ b/src/drivers/drv_baro.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Barometric pressure sensor driver interface.
+ * @file drv_baro.h
+ *
+ * Barometric pressure sensor driver interface.
  */
 
 #ifndef _DRV_BARO_H
diff --git a/src/drivers/drv_gps.h b/src/drivers/drv_gps.h
index 398cf48701..06e3535b30 100644
--- a/src/drivers/drv_gps.h
+++ b/src/drivers/drv_gps.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file GPS driver interface.
+ * @file drv_gps.h
+ *
+ * GPS driver interface.
  */
 
 #ifndef _DRV_GPS_H
diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h
index fefcae50b9..2ae8c70582 100644
--- a/src/drivers/drv_gyro.h
+++ b/src/drivers/drv_gyro.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Gyroscope driver interface.
+ * @file drv_gyro.h
+ *
+ * Gyroscope driver interface.
  */
 
 #ifndef _DRV_GYRO_H
diff --git a/src/drivers/drv_rc_input.h b/src/drivers/drv_rc_input.h
index 4decc324ef..78ffad9d7b 100644
--- a/src/drivers/drv_rc_input.h
+++ b/src/drivers/drv_rc_input.h
@@ -32,8 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file R/C input interface.
+ * @file drv_rc_input.h
  *
+ * R/C input interface.
  */
 
 #ifndef _DRV_RC_INPUT_H
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index 3a89cab9dd..8e8b2c1b95 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -32,7 +32,9 @@
  ****************************************************************************/
 
 /**
- * @file Common sensor API and ioctl definitions.
+ * @file drv_sensor.h
+ *
+ * Common sensor API and ioctl definitions.
  */
 
 #ifndef _DRV_SENSOR_H
diff --git a/src/drivers/drv_tone_alarm.h b/src/drivers/drv_tone_alarm.h
index 2fab37dd21..cb5fd53a5f 100644
--- a/src/drivers/drv_tone_alarm.h
+++ b/src/drivers/drv_tone_alarm.h
@@ -31,7 +31,9 @@
  *
  ****************************************************************************/
 
-/*
+/**
+ * @file drv_tone_alarm.h
+ *
  * Driver for the PX4 audio alarm port, /dev/tone_alarm.
  *
  * The tone_alarm driver supports a set of predefined "alarm"