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

(limited to 'src')

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(+)

(limited to 'src')

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(-)

(limited to 'src')

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 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(-)

(limited to 'src')

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(-)

(limited to 'src')

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(-)

(limited to 'src')

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 0f67c5cbb0f69e5b9dda4e4e75120e63e466e1f8 Mon Sep 17 00:00:00 2001
From: Alexander Lourier <aml@rulezz.ru>
Date: Fri, 18 Oct 2013 03:47:15 +0400
Subject: Parameters list generator

---
 Tools/px4params/README.md            |   9 ++
 Tools/px4params/dokuwikiout.py       |  27 +++++
 Tools/px4params/output.py            |   5 +
 Tools/px4params/parser.py            | 220 +++++++++++++++++++++++++++++++++++
 Tools/px4params/px_process_params.py |  61 ++++++++++
 Tools/px4params/scanner.py           |  34 ++++++
 Tools/px4params/xmlout.py            |  22 ++++
 src/modules/sensors/sensor_params.c  |  26 +++++
 8 files changed, 404 insertions(+)
 create mode 100644 Tools/px4params/README.md
 create mode 100644 Tools/px4params/dokuwikiout.py
 create mode 100644 Tools/px4params/output.py
 create mode 100644 Tools/px4params/parser.py
 create mode 100755 Tools/px4params/px_process_params.py
 create mode 100644 Tools/px4params/scanner.py
 create mode 100644 Tools/px4params/xmlout.py

(limited to 'src')

diff --git a/Tools/px4params/README.md b/Tools/px4params/README.md
new file mode 100644
index 000000000..a23b44799
--- /dev/null
+++ b/Tools/px4params/README.md
@@ -0,0 +1,9 @@
+h1. PX4 Parameters Processor
+
+It's designed to scan PX4 source codes, find declarations of tunable parameters,
+and generate the list in various formats.
+
+Currently supported formats are:
+
+* XML for the parametric UI generator
+* Human-readable description in DokuWiki format
diff --git a/Tools/px4params/dokuwikiout.py b/Tools/px4params/dokuwikiout.py
new file mode 100644
index 000000000..33f76b415
--- /dev/null
+++ b/Tools/px4params/dokuwikiout.py
@@ -0,0 +1,27 @@
+import output
+
+class DokuWikiOutput(output.Output):
+    def Generate(self, groups):
+        result = ""
+        for group in groups:
+            result += "==== %s ====\n\n" % group.GetName()
+            for param in group.GetParams():
+                code = param.GetFieldValue("code")
+                name = param.GetFieldValue("short_desc")
+                if code != name:
+                    name = "%s (%s)" % (name, code)
+                result += "=== %s ===\n\n" % name
+                long_desc = param.GetFieldValue("long_desc")
+                if long_desc is not None:
+                    result += "%s\n\n" % long_desc
+                min_val = param.GetFieldValue("min")
+                if min_val is not None:
+                    result += "* Minimal value: %s\n" % min_val
+                max_val = param.GetFieldValue("max")
+                if max_val is not None:
+                    result += "* Maximal value: %s\n" % max_val
+                def_val = param.GetFieldValue("default")
+                if def_val is not None:
+                    result += "* Default value: %s\n" % def_val
+                result += "\n"
+        return result
diff --git a/Tools/px4params/output.py b/Tools/px4params/output.py
new file mode 100644
index 000000000..c09246871
--- /dev/null
+++ b/Tools/px4params/output.py
@@ -0,0 +1,5 @@
+class Output(object):
+    def Save(self, groups, fn):
+        data = self.Generate(groups)
+        with open(fn, 'w') as f:
+            f.write(data)
diff --git a/Tools/px4params/parser.py b/Tools/px4params/parser.py
new file mode 100644
index 000000000..251be672f
--- /dev/null
+++ b/Tools/px4params/parser.py
@@ -0,0 +1,220 @@
+import sys
+import re
+
+class ParameterGroup(object):
+    """
+    Single parameter group
+    """
+    def __init__(self, name):
+        self.name = name
+        self.params = []
+
+    def AddParameter(self, param):
+        """
+        Add parameter to the group
+        """
+        self.params.append(param)
+
+    def GetName(self):
+        """
+        Get parameter group name
+        """
+        return self.name
+
+    def GetParams(self):
+        """
+        Returns the parsed list of parameters. Every parameter is a Parameter
+        object. Note that returned object is not a copy. Modifications affect
+        state of the parser.
+        """
+        return sorted(self.params,
+                cmp=lambda x, y: cmp(x.GetFieldValue("code"),
+                    y.GetFieldValue("code")))
+
+class Parameter(object):
+    """
+    Single parameter
+    """
+
+    # Define sorting order of the fields
+    priority = {
+        "code": 10,
+        "type": 9,
+        "short_desc": 8,
+        "long_desc": 7,
+        "default": 6,
+        "min": 5,
+        "max": 4,
+        # all others == 0 (sorted alphabetically)
+    }
+
+    def __init__(self):
+        self.fields = {}
+
+    def SetField(self, code, value):
+        """
+        Set named field value
+        """
+        self.fields[code] = value
+
+    def GetFieldCodes(self):
+        """
+        Return list of existing field codes in convenient order
+        """
+        return sorted(self.fields.keys(),
+                cmp=lambda x, y: cmp(self.priority.get(y, 0),
+                    self.priority.get(x, 0)) or cmp(x, y))
+
+    def GetFieldValue(self, code):
+        """
+        Return value of the given field code or None if not found.
+        """
+        return self.fields.get(code)
+
+class Parser(object):
+    """
+    Parses provided data and stores all found parameters internally.
+    """
+
+    re_split_lines = re.compile(r'[\r\n]+')
+    re_comment_start = re.compile(r'^\/\*\*')
+    re_comment_content = re.compile(r'^\*\s*(.*)') 
+    re_comment_tag = re.compile(r'@([a-zA-Z][a-zA-Z0-9_]*)\s*(.*)')
+    re_comment_end = re.compile(r'(.*?)\s*\*\/')
+    re_parameter_definition = re.compile(r'PARAM_DEFINE_([A-Z_][A-Z0-9_]*)\s*\(([A-Z_][A-Z0-9_]*)\s*,\s*([^ ,\)]+)\s*\)\s*;')
+    re_cut_type_specifier = re.compile(r'[a-z]+$')
+    re_is_a_number = re.compile(r'^-?[0-9\.]')
+    re_remove_dots = re.compile(r'\.+$')
+
+    valid_tags = set(["min", "max", "group"])
+
+    # Order of parameter groups
+    priority = {
+        # All other groups = 0 (sort alphabetically)
+        "Miscellaneous": -10
+    }
+
+    def __init__(self):
+        self.param_groups = {}
+
+    def GetSupportedExtensions(self):
+        """
+        Returns list of supported file extensions that can be parsed by this
+        parser.
+        """
+        return ["cpp", "c"]
+
+    def Parse(self, contents):
+        """
+        Incrementally parse program contents and append all found parameters
+        to the list.
+        """
+        # This code is essentially a comment-parsing grammar. "state"
+        # represents parser state. It contains human-readable state
+        # names.
+        state = None
+        for line in self.re_split_lines.split(contents):
+            line = line.strip()
+            # Ignore empty lines
+            if line == "":
+                continue
+            if self.re_comment_start.match(line):
+                state = "wait-short"
+                short_desc = None
+                long_desc = None
+                tags = {}
+            elif state is not None and state != "comment-processed":
+                m = self.re_comment_end.search(line)
+                if m:
+                    line = m.group(1)
+                    last_comment_line = True
+                else:
+                    last_comment_line = False
+                m = self.re_comment_content.match(line)
+                if m:
+                    comment_content = m.group(1)
+                    if comment_content == "":
+                        # When short comment ends with empty comment line,
+                        # start waiting for the next part - long comment.
+                        if state == "wait-short-end":
+                            state = "wait-long"
+                    else:
+                        m = self.re_comment_tag.match(comment_content)
+                        if m:
+                            tag, desc = m.group(1, 2)
+                            tags[tag] = desc
+                            current_tag = tag
+                            state = "wait-tag-end"
+                        elif state == "wait-short":
+                            # Store first line of the short description
+                            short_desc = comment_content
+                            state = "wait-short-end"
+                        elif state == "wait-short-end":
+                            # Append comment line to the short description
+                            short_desc += "\n" + comment_content
+                        elif state == "wait-long":
+                            # Store first line of the long description
+                            long_desc = comment_content
+                            state = "wait-long-end"
+                        elif state == "wait-long-end":
+                            # Append comment line to the long description
+                            long_desc += "\n" + comment_content
+                        elif state == "wait-tag-end":
+                            # Append comment line to the tag text
+                            tags[current_tag] += "\n" + comment_content
+                        else:
+                            raise AssertionError(
+                                    "Invalid parser state: %s" % state)
+                elif not last_comment_line:
+                    # Invalid comment line (inside comment, but not starting with
+                    # "*" or "*/". Reset parsed content.
+                    state = None
+                if last_comment_line:
+                    state = "comment-processed"
+            else:
+                # Non-empty line outside the comment
+                m = self.re_parameter_definition.match(line)
+                if m:
+                    tp, code, defval = m.group(1, 2, 3)
+                    # Remove trailing type specifier from numbers: 0.1f => 0.1
+                    if self.re_is_a_number.match(defval):
+                        defval = self.re_cut_type_specifier.sub('', defval)
+                    param = Parameter()
+                    param.SetField("code", code)
+                    param.SetField("short_desc", code)
+                    param.SetField("type", tp)
+                    param.SetField("default", defval)
+                    # If comment was found before the parameter declaration,
+                    # inject its data into the newly created parameter.
+                    group = "Miscellaneous"
+                    if state == "comment-processed":
+                        if short_desc is not None:
+                            param.SetField("short_desc",
+                                    self.re_remove_dots.sub('', short_desc))
+                        if long_desc is not None:
+                            param.SetField("long_desc", long_desc)
+                        for tag in tags:
+                            if tag == "group":
+                                group = tags[tag]
+                            elif tag not in self.valid_tags:
+                                sys.stderr.write("Skipping invalid"
+                                        "documentation tag: '%s'\n" % tag)
+                            else:
+                                param.SetField(tag, tags[tag])
+                    # Store the parameter
+                    if group not in self.param_groups:
+                        self.param_groups[group] = ParameterGroup(group)
+                    self.param_groups[group].AddParameter(param)
+                # Reset parsed comment.
+                state = None
+
+    def GetParamGroups(self):
+        """
+        Returns the parsed list of parameters. Every parameter is a Parameter
+        object. Note that returned object is not a copy. Modifications affect
+        state of the parser.
+        """
+        return sorted(self.param_groups.values(),
+                cmp=lambda x, y: cmp(self.priority.get(y.GetName(), 0),
+                    self.priority.get(x.GetName(), 0)) or cmp(x.GetName(),
+                        y.GetName()))
diff --git a/Tools/px4params/px_process_params.py b/Tools/px4params/px_process_params.py
new file mode 100755
index 000000000..cdf5aba7c
--- /dev/null
+++ b/Tools/px4params/px_process_params.py
@@ -0,0 +1,61 @@
+#!/usr/bin/env python
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# PX4 paramers processor (main executable file)
+#
+# It scans src/ subdirectory of the project, collects all parameters
+# definitions, and outputs list of parameters in XML and DokuWiki formats.
+#
+
+import scanner
+import parser
+import xmlout
+import dokuwikiout
+
+# Initialize parser
+prs = parser.Parser()
+
+# Scan directories, and parse the files
+sc = scanner.Scanner()
+sc.ScanDir("../../src", prs)
+output = prs.GetParamGroups()
+
+# Output into XML
+out = xmlout.XMLOutput()
+out.Save(output, "parameters.xml")
+
+# Output into DokuWiki
+out = dokuwikiout.DokuWikiOutput()
+out.Save(output, "parameters.wiki")
diff --git a/Tools/px4params/scanner.py b/Tools/px4params/scanner.py
new file mode 100644
index 000000000..b5a1af47c
--- /dev/null
+++ b/Tools/px4params/scanner.py
@@ -0,0 +1,34 @@
+import os
+import re
+
+class Scanner(object):
+    """
+    Traverses directory tree, reads all source files, and passes their contents
+    to the Parser.
+    """
+
+    re_file_extension = re.compile(r'\.([^\.]+)$')
+
+    def ScanDir(self, srcdir, parser):
+        """
+        Scans provided path and passes all found contents to the parser using
+        parser.Parse method.
+        """
+        extensions = set(parser.GetSupportedExtensions())
+        for dirname, dirnames, filenames in os.walk(srcdir):
+            for filename in filenames:
+                m = self.re_file_extension.search(filename)
+                if m:
+                    ext = m.group(1)
+                    if ext in extensions:
+                        path = os.path.join(dirname, filename)
+                        self.ScanFile(path, parser)
+
+    def ScanFile(self, path, parser):
+        """
+        Scans provided file and passes its contents to the parser using
+        parser.Parse method.
+        """
+        with open(path, 'r') as f:
+            contents = f.read()
+        parser.Parse(contents)
diff --git a/Tools/px4params/xmlout.py b/Tools/px4params/xmlout.py
new file mode 100644
index 000000000..d56802b15
--- /dev/null
+++ b/Tools/px4params/xmlout.py
@@ -0,0 +1,22 @@
+import output
+from xml.dom.minidom import getDOMImplementation
+
+class XMLOutput(output.Output):
+    def Generate(self, groups):
+        impl = getDOMImplementation()
+        xml_document = impl.createDocument(None, "parameters", None)
+        xml_parameters = xml_document.documentElement
+        for group in groups:
+            xml_group = xml_document.createElement("group")
+            xml_group.setAttribute("name", group.GetName())
+            xml_parameters.appendChild(xml_group)
+            for param in group.GetParams():
+                xml_param = xml_document.createElement("parameter")
+                xml_group.appendChild(xml_param)
+                for code in param.GetFieldCodes():
+                    value = param.GetFieldValue(code)
+                    xml_field = xml_document.createElement(code)
+                    xml_param.appendChild(xml_field)
+                    xml_value = xml_document.createTextNode(value)
+                    xml_field.appendChild(xml_value)
+        return xml_document.toprettyxml(indent="    ", newl="\n", encoding="utf-8")
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 4d719e0e1..8875f65fc 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -44,8 +44,34 @@
 
 #include <systemlib/param/param.h>
 
+/**
+ * Gyro X offset FIXME
+ *
+ * This is an X-axis offset for the gyro.
+ * Adjust it according to the calibration data.
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Gyro Config
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
+
+/**
+ * Gyro Y offset FIXME with dot.
+ *
+ * @min -10.0
+ * @max 10.0
+ * @group Gyro Config
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
+
+/**
+ * Gyro Z offset FIXME
+ *
+ * @min -5.0
+ * @max 5.0
+ * @group Gyro Config
+ */
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
 
 PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
-- 
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(-)

(limited to 'src')

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 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(-)

(limited to 'src')

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(-)

(limited to 'src')

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(+)

(limited to 'src')

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(-)

(limited to 'src')

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(-)

(limited to 'src')

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

(limited to 'src')

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(-)

(limited to 'src')

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(-)

(limited to 'src')

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(-)

(limited to 'src')

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(-)

(limited to 'src')

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

(limited to 'src')

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(-)

(limited to 'src')

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

(limited to 'src')

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(-)

(limited to 'src')

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(-)

(limited to 'src')

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 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(-)

(limited to 'src')

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(-)

(limited to 'src')

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(-)

(limited to 'src')

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(-)

(limited to 'src')

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(-)

(limited to 'src')

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(-)

(limited to 'src')

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 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(-)

(limited to 'src')

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 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(-)

(limited to 'src')

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