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: 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 ec8033202..edb21c7ab 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 0e88da054..9f709e459 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 000000000..c6a9ba638
--- /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_ */
-- 
cgit v1.2.3


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: Piping through manual control channels

---
 src/modules/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 ae9aaa2da..eb67874db 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 44f8f664b..60a211817 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 3fcacaf8f..630131e1e 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);
+		}
 	}
 
 }
-- 
cgit v1.2.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: 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 93e76184d..e0813b031 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 75e82bb00..0ec663e35 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 20cb88b91..c73cb2a62 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 ebcb66b24..17ff71151 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 95beb8927..f07c34ac8 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 9f81e1dc3..610868354 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 981466704..f0aa6650d 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 b5e38ce9e..f8f9f0e4d 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 8e8d122ad..26b40b9e9 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 dfdf1d58e..cd9a9cfab 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 2a4a0f341..520aba635 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 81b4af30b..58e6af74b 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 12a3bee20..fa21c8012 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 4d719e0e1..682cb3d81 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);
-- 
cgit v1.2.3


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: 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 e0813b031..93e76184d 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
-- 
cgit v1.2.3


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: 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/fw_pos_control_l1_main.cpp   | 32 +++++++++++++++++-----
 .../fw_pos_control_l1/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 2ae6b28bb..4a98c8e97 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 cd4a0d58e..3697af225 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 d210ec712..9b64cb047 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);
 
-- 
cgit v1.2.3


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: More improvements on landing

---
 .../fw_pos_control_l1/fw_pos_control_l1_main.cpp   | 72 +++++++++++++++++++---
 .../fw_pos_control_l1/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 3697af225..348a17ba6 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 9b64cb047..c39df9ae3 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);
 
-- 
cgit v1.2.3


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: Almost perfect landing approach, needs touch-down fine tuning

---
 .../fw_pos_control_l1/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 348a17ba6..219c6ffa6 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 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;
 
-					// /* set the speed weight to 0.0 to push the system to control altitude with pitch */
-					// _tecs.set_speed_weight(0.0f);
+				if (altitude_error > -5.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 ddad5f0df..adaf81404 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;
-- 
cgit v1.2.3


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: 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 000000000..0918edcac
Binary files /dev/null and b/Documentation/fixed_wing_control.odg differ
diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg
deleted file mode 100644
index 69910c677..000000000
Binary files a/Documentation/l1_control.odg and /dev/null differ
-- 
cgit v1.2.3


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: Fixes, but approach needs proper design

---
 .../fw_pos_control_l1/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 219c6ffa6..9ed74f0ca 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 adaf81404..7e4a2688f 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;
 
-- 
cgit v1.2.3


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: 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 000000000..0f0bb05ce
--- /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 93e76184d..ced875001 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
-- 
cgit v1.2.3


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: 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 000000000..1c85e04f2
--- /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 ced875001..fd588017f 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
 	#
-- 
cgit v1.2.3


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: 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 e645d9d54..2bfaed76c 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
-- 
cgit v1.2.3


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: 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 ea87b37d9..34a45c739 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;
 }
-- 
cgit v1.2.3


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: 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 34a45c739..d49211b7b 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;
-- 
cgit v1.2.3


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: 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 d49211b7b..727c86e02 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;
 			}
 		}
-- 
cgit v1.2.3


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: 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 | 178 +++++++++++++----------------
 1 file changed, 78 insertions(+), 100 deletions(-)

diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 369e6da62..b6de5141f 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;
-
-
-	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 */
+	gyro_scale.x_offset /= calibration_count;
+	gyro_scale.y_offset /= calibration_count;
+	gyro_scale.z_offset /= calibration_count;
 
-	} 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 (!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;
+	}
 
-		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!");
-		}
+	/* beep on calibration end */
+	mavlink_log_info(mavlink_fd, "scale calibration done.");
+	tune_neutral();
 
-		/* 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],
-		};
+#endif
 
-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
-			warn("WARNING: failed to set scale / offsets for gyro");
+	/* 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!");
+	}
 
-		close(fd);
+	/* apply new scaling and offsets */
+	fd = open(GYRO_DEVICE_PATH, 0);
 
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
+		warn("WARNING: failed to apply new scale for gyro");
 
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
-		}
+	close(fd);
 
-		mavlink_log_info(mavlink_fd, "gyro calibration done");
+	/* auto-save to EEPROM */
+	int save_ret = param_save_default();
 
-		/* 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 (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;
 }
-- 
cgit v1.2.3


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: 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 8243748dc..c51a6de08 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;
 
-- 
cgit v1.2.3


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: 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 46640f3c5..862abde92 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 bb589cb9f..71986c3a3 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 000000000..102711aaf
--- /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 000000000..b65226cf1
--- /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 000000000..97c6a0907
--- /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 cfa7d9e8a..c9bfedbda 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 91d75121e..554dfcb08 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 3fcacaf8f..1b79de8fd 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()
 {
-- 
cgit v1.2.3


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: 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 c9bfedbda..b6aa64aa4 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;
 }
 
-- 
cgit v1.2.3


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: 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 102711aaf..f5f59a2dc 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 b65226cf1..b078562c2 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 97c6a0907..85c63c0fc 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 b6aa64aa4..4880af907 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;
 
-- 
cgit v1.2.3


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: 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 b6de5141f..e1d6e8340 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;
 }
-- 
cgit v1.2.3


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: calibration: bugs fixed, mavlink messages cleanup

---
 src/drivers/drv_accel.h                            |   2 +-
 .../commander/accelerometer_calibration.cpp        | 181 ++++++++++--------
 src/modules/commander/commander.cpp                |  54 +++---
 src/modules/commander/gyro_calibration.cpp         | 211 ++++++++++++---------
 4 files changed, 252 insertions(+), 196 deletions(-)

diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h
index eff5e7349..8a4f68428 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 4880af907..d11d7eb5d 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;
 		}
+	}
 
+	if (res == OK) {
+		/* apply new scaling and offsets */
 		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");
-
+		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 */
-		int save_ret = param_save_default();
+		res = param_save_default();
 
-		if (save_ret != 0) {
-			warn("WARNING: auto-save of params to storage failed");
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters");
 		}
+	}
 
-		mavlink_log_info(mavlink_fd, "accel calibration done");
-		return OK;
+	if (res == OK) {
+		mavlink_log_info(mavlink_fd, "accel calibration: done");
 
 	} else {
-		/* measurements error */
-		mavlink_log_info(mavlink_fd, "accel calibration aborted");
-		return ERROR;
+		mavlink_log_info(mavlink_fd, "accel calibration: failed");
 	}
 
-	/* exit accel calibration mode */
+	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 2ef509980..9545ef171 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 e1d6e8340..219ae6cb2 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,
 	};
 
-	/* subscribe to gyro sensor topic */
-	int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
-	struct gyro_report gyro_report;
+	int res = OK;
 
 	/* 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");
-
+	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");
+	}
 
-	/*** --- 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_gyro;
-		fds[0].events = POLLIN;
-
-		int poll_ret = poll(fds, 1, 1000);
+	/* subscribe to gyro sensor topic */
+	int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro));
+	struct gyro_report gyro_report;
 
-		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 (res == OK) {
+		/* 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_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 calibration: progress <%u>", (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;
 	}
 
-	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) {
+		/* 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;
+		}
 	}
 
-	/* beep on 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!");
+	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;
+		}
 	}
 
-
-	/*** --- SCALING --- ***/
 #if 0
+	/* beep on offset calibration end */
+	mavlink_log_info(mavlink_fd, "gyro offset calibration done");
+	tune_neutral();
+
+	/* 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!");
-	}
+	close(sub_sensor_gyro);
 
-	/* apply new scaling and offsets */
-	fd = open(GYRO_DEVICE_PATH, 0);
+	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 (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
-		warn("WARNING: failed to apply new scale for gyro");
+	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);
 
-	close(fd);
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to apply new params for gyro");
+		}
+	}
 
-	/* auto-save to EEPROM */
-	int save_ret = param_save_default();
+	if (res == OK) {
+		/* auto-save to EEPROM */
+		res = 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;
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to save parameters");
+		}
 	}
 
-	mavlink_log_info(mavlink_fd, "gyro calibration done.");
+	if (res == OK) {
+		mavlink_log_info(mavlink_fd, "gyro calibration: done");
 
-	close(sub_sensor_gyro);
-	return OK;
+	} else {
+		mavlink_log_info(mavlink_fd, "gyro calibration: failed");
+	}
+
+	return res;
 }
-- 
cgit v1.2.3


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: 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          | 276 ++++++++++-----------
 4 files changed, 239 insertions(+), 171 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 d11d7eb5d..49a8d6b33 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 000000000..fd8b8564d
--- /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 219ae6cb2..30cd0d48d 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 b0d4266be..09f4104f8 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;
 
-	/* calibrate range */
-	if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) {
-		warnx("failed to calibrate scale");
-	}
+	/* erase old calibration */
+	int fd = open(MAG_DEVICE_PATH, O_RDONLY);
+	res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
 
-	close(fd);
+	if (res != OK) {
+		mavlink_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG);
+	}
 
-	mavlink_log_info(mavlink_fd, "mag cal progress <20> percent");
+	if (res == OK) {
+		/* calibrate range */
+		res = ioctl(fd, MAGIOCCALIBRATE, fd);
 
-	/* calibrate offsets */
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale");
+		}
+	}
 
-	// uint64_t calibration_start = hrt_absolute_time();
+	close(fd);
 
-	uint64_t axis_deadline = hrt_absolute_time();
-	uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval;
+	float *x;
+	float *y;
+	float *z;
 
-	const char axislabels[3] = { 'X', 'Y', 'Z'};
-	int axis_index = -1;
+	if (res == OK) {
+		/* allocate memory */
+		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
 
-	float *x = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *y = (float *)malloc(sizeof(float) * calibration_maxcount);
-	float *z = (float *)malloc(sizeof(float) * calibration_maxcount);
+		x = (float *)malloc(sizeof(float) * calibration_maxcount);
+		y = (float *)malloc(sizeof(float) * calibration_maxcount);
+		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);
 
-		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;
 
-			x[calibration_counter] = mag.x;
-			y[calibration_counter] = mag.y;
-			z[calibration_counter] = mag.z;
+				calibration_counter++;
 
-			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);
+				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++;
+			}
 
-		} 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;
+			if (poll_errcount > 1000) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG);
+				res = ERROR;
+				break;
+			}
 		}
 
-
+		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);
 
-		struct mag_scale mscale;
+	if (z != NULL)
+		free(z);
 
-		if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to get scale / offsets for mag");
+	if (res == OK) {
+		/* apply calibration and set parameters */
+		struct mag_scale mscale;
 
-		mscale.x_offset = sphere_x;
-		mscale.y_offset = sphere_y;
-		mscale.z_offset = sphere_z;
+		fd = open(MAG_DEVICE_PATH, 0);
+		res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale);
 
-		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
-			warn("WARNING: failed to set scale / offsets for mag");
+		if (res != OK) {
+			mavlink_log_critical(mavlink_fd, "ERROR: failed to get current calibration");
+		}
 
-		close(fd);
+		if (res == OK) {
+			mscale.x_offset = sphere_x;
+			mscale.y_offset = sphere_y;
+			mscale.z_offset = sphere_z;
 
-		/* announce and set new offset */
+			res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale);
 
-		if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
-			warnx("Setting X mag offset failed!\n");
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG);
+			}
 		}
 
-		if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
-			warnx("Setting Y mag offset failed!\n");
-		}
+		close(fd);
 
-		if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
-			warnx("Setting Z mag offset failed!\n");
-		}
+		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_XSCALE"), &(mscale.x_scale))) {
-			warnx("Setting X mag scale failed!\n");
-		}
+			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
+				res = ERROR;
 
-		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_ZOFF"), &(mscale.z_offset)))
+				res = ERROR;
 
-		if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
-			warnx("Setting Z mag scale failed!\n");
-		}
+			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;
 
-		mavlink_log_info(mavlink_fd, "mag cal progress <90> percent");
+			if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
+				res = ERROR;
 
-		/* auto-save to EEPROM */
-		int save_ret = param_save_default();
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
+			}
 
-		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;
+			mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
 		}
 
-		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);
+		if (res == OK) {
+			/* auto-save to EEPROM */
+			res = param_save_default();
 
-		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);
+			if (res != OK) {
+				mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+			}
+		}
 
-		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, "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);
 
-		mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
-		mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
+		if (res == OK) {
+			mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
 
-		close(sub_mag);
-		return OK;
-		/* third beep by cal end routine */
+		} else {
+			mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		}
 
-	} else {
-		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
-		close(sub_mag);
-		return ERROR;
+		return res;
 	}
 }
-- 
cgit v1.2.3


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: 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 49a8d6b33..5eeca5a1a 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 */
-- 
cgit v1.2.3


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: 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 46640f3c5..3cccc8447 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 bb589cb9f..a2027ded9 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 000000000..88b22e72a
--- /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 000000000..27095b357
--- /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 000000000..1abecf198
--- /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 000000000..a61c646fc
--- /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
-- 
cgit v1.2.3


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: 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 88b22e72a..aecc511ae 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 27095b357..619c2289c 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 a61c646fc..f37f71aac 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);
 
-- 
cgit v1.2.3


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: 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 4b7ed5286..ffe7f695b 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 fd588017f..3a458286e 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 aecc511ae..3bbd7f48f 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 619c2289c..e9f35cf95 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 f37f71aac..44ed03254 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\n", deviceName, address);
+		printf("device:\t%s\taddress:\t%d\tpulses per rev:\t%ld\n",
+			deviceName, address, pulsesPerRev);
 
-		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;
 
-- 
cgit v1.2.3


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: 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 ffe7f695b..fb9dec043 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 3a458286e..424787d54 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
-- 
cgit v1.2.3


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: 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 3bbd7f48f..73bd5dada 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()
-- 
cgit v1.2.3


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: 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 3cccc8447..85ac3f546 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 a2027ded9..c0972be9e 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 b1dc39445..96a443c6e 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;
 		}
-- 
cgit v1.2.3


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: 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 73bd5dada..d65a9be36 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;
 }
-- 
cgit v1.2.3


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: 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 2ef509980..db758c386 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 60d6473b8..b4350cc24 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 e2238d151..e70b83cce 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 e9c5f1a2c..1c58a2db6 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
+}
-- 
cgit v1.2.3


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: 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 | 85 ++++++++++++++++++-------------------
 1 file changed, 42 insertions(+), 43 deletions(-)

diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index ccdb2ea38..398657dd7 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);
+	int res;
+	int fd;
 
-		if (result != OK)
-			warnx("unlinking file %s failed.", param_get_default_file());
-	}
+	const char *filename = param_get_default_file();
+	const char *filename_tmp = malloc(strlen(filename) + 5);
+	sprintf(filename_tmp, "%s.tmp", filename);
 
-	/* create the file */
-	int fd;
+	/* 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);
 
-	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);
 		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;
-	}
+			if (res != OK)
+				warnx("failed to write parameters to file: %s", filename_tmp);
+		}
 
-	do {
-		result = param_export(fd, false);
+		close(fd);
+	}
 
-		if (result != OK) {
-			retries++;
-			usleep(1000 * retries);
-		}
+	if (res == OK) {
+		/* delete parameters file */
+		res = unlink(filename);
 
-	} while (result != 0 && retries < 10);
+		if (res != OK && errno == ENOENT)
+			res = OK;
 
+		if (res != OK)
+			warn("failed to delete parameters file: %s", filename);
+	}
 
-	close(fd);
+	if (res == OK) {
+		/* rename temp file to parameters */
+		res = rename(filename_tmp, filename);
 
-	if (result != OK) {
-		warn("error exporting parameters to '%s'", param_get_default_file());
-		(void)unlink(param_get_default_file());
-		return result;
+		if (res != OK)
+			warn("failed to rename %s to %s", filename_tmp, filename);
 	}
 
-	return 0;
+	free(filename_tmp);
+
+	return res;
 }
 
 /**
-- 
cgit v1.2.3


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: 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 61a54170a..f94875d5b 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));
+}
+
+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;
 
-	fsync(fd);
+		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 5c175ef7e..dc5e6c8bd 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 9f709e459..90093a407 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);
-- 
cgit v1.2.3


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: 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 7fefc5908..5b1e55e22 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
-- 
cgit v1.2.3


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: Rascal (AERT) hil startup script

---
 ROMFS/px4fmu_common/init.d/1000_rc_fw.hil          |  92 ------------------
 ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil |  92 ++++++++++++++++++
 .../px4fmu_common/init.d/1004_rc_fw_Rascal110.hil  | 103 +++++++++++++++++++++
 ROMFS/px4fmu_common/init.d/rcS                     |   8 +-
 4 files changed, 202 insertions(+), 93 deletions(-)
 delete mode 100644 ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
 create mode 100644 ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
 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.hil
deleted file mode 100644
index 5e4028bbb..000000000
--- a/ROMFS/px4fmu_common/init.d/1000_rc_fw.hil
+++ /dev/null
@@ -1,92 +0,0 @@
-#!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)
-#
-mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
-fw_pos_control_l1 start
-fw_att_control start
-
-echo "[HIL] setup done, running"
-
diff --git a/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
new file mode 100644
index 000000000..5e4028bbb
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil
@@ -0,0 +1,92 @@
+#!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)
+#
+mixer load /dev/pwm_output /etc/mixers/FMU_AET.mix
+fw_pos_control_l1 start
+fw_att_control start
+
+echo "[HIL] setup done, running"
+
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 000000000..8c5e4b6a8
--- /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 424787d54..94c01419e 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
-- 
cgit v1.2.3


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: 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 084671ae2..9d8ad084e 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 ee45d46ac..b3003fc1b 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;
 }
 
-- 
cgit v1.2.3


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: 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 000000000..73cf39575
--- /dev/null
+++ b/Tools/px4params/.gitignore
@@ -0,0 +1,2 @@
+parameters.wiki
+parameters.xml
\ No newline at end of file
-- 
cgit v1.2.3


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: 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 +++++++++++
 src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp |  5 +++++
 src/modules/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 daf136d49..196ded26c 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 5a17346cb..7a3c42a92 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 cd4a0d58e..f09e44b39 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 d210ec712..bf4b3b32a 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);
 
 
-- 
cgit v1.2.3


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: 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 000000000..432cbb14e
--- /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 94c01419e..9d0800eb1 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
-- 
cgit v1.2.3


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: 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 432cbb14e..cd7677112 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
-- 
cgit v1.2.3


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: 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 196ded26c..27d76f959 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 */
-- 
cgit v1.2.3


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: 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 10aeb5c9f..79b6546b3 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 e70b3fe88..e28106971 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 94d7407df..e9adc8cd6 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);
-- 
cgit v1.2.3


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: 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 236cb918d..43318ca84 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 */
-- 
cgit v1.2.3


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: 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 8bed8a8df..60601e22c 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 {
-- 
cgit v1.2.3


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: 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 9814a7bcc..44a144296 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);
 					}
-- 
cgit v1.2.3


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: 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 8a4f68428..7d93ed938 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 7bb9ee2af..78f31495d 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 aa251889f..e2f0137ae 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 398cf4870..06e3535b3 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 fefcae50b..2ae8c7058 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 4decc324e..78ffad9d7 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 3a89cab9d..8e8b2c1b9 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 2fab37dd2..cb5fd53a5 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"
-- 
cgit v1.2.3