Merge branch 'master' into ashtech_hi_freq

2026-06-08 10:50:19 +08:00 · 2014-10-08 11:56:58 +04:00
parent 6bf005e901 a06a278a18
commit 2de50ab35b
68 changed files with 3372 additions and 626 deletions
@@ -9,4 +9,3 @@ sh /etc/init.d/rc.fw_defaults

 set MIXER Viper

-set FAILSAFE "-c567 -p 1000"
@@ -21,6 +21,12 @@
 # Simulation setups
 #

+if param compare SYS_AUTOSTART 901
+then
+	sh /etc/init.d/901_bottle_drop_test.hil
+	set MODE custom
+fi
+
 if param compare SYS_AUTOSTART 1000
 then
 	sh /etc/init.d/1000_rc_fw_easystar.hil
@@ -13,3 +13,5 @@ ekf_att_pos_estimator start
 #
 fw_att_control start
 fw_pos_control_l1 start
+
+bottle_drop start
@@ -68,6 +68,11 @@ else
 	fi
 fi

+# Check for flow sensor
+if px4flow start
+then
+fi
+
 #
 # Start the sensor collection task.
 # IMPORTANT: this also loads param offsets
@@ -52,21 +52,18 @@ M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000

-Gimbal / flaps / payload mixer for last four channels
+Inputs to the mixer come from channel group 2 (payload), channels 0
+(bay servo 1), 1 (bay servo 2) and 3 (drop release).
 -----------------------------------------------------

 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 4  10000  10000      0 -10000  10000
+S: 2 0  10000  10000      0 -10000  10000

 M: 1
 O:      10000  10000      0 -10000  10000
-S: 0 5  10000  10000      0 -10000  10000
+S: 2 1  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
+S: 2 2 -10000 -10000      0 -10000  10000
@@ -66,6 +66,6 @@ S: 2 1  10000  10000      0 -10000  10000

 M: 1
 O:      10000  10000      0 -10000  10000
-S: 2 2 -10000 -10000      0 -10000  10000
+S: 2 2  -8000  -8000      0 -10000  10000


@@ -75,3 +75,33 @@ if [ -f /fs/microsd/mount_test_cmds.txt ]
 then
 	tests mount
 fi
+
+#
+# Run unit tests at board boot, reporting failure as needed.
+# Add new unit tests using the same pattern as below.
+#
+
+set unit_test_failure 0
+
+if mavlink_tests
+then
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} mavlink_tests"
+fi
+
+if commander_tests
+then
+else
+	set unit_test_failure 1
+	set unit_test_failure_list "${unit_test_failure_list} commander_tests"
+fi
+
+if [ $unit_test_failure == 0 ]
+then
+	echo
+	echo "All Unit Tests PASSED"
+else
+	echo
+	echo "Some Unit Tests FAILED:${unit_test_failure_list}"
+fi
@@ -1,4 +1,6 @@
 ./obj/*
 mixer_test
+sf0x_test
 sbus2_test
 autodeclination_test
+st24_test
@@ -3,7 +3,7 @@ CC=g++
 CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \
 	-I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm

-all: mixer_test sbus2_test autodeclination_test
+all: mixer_test sbus2_test autodeclination_test st24_test sf0x_test

 MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \
 		../../src/systemcmds/tests/test_conv.cpp \
@@ -20,7 +20,16 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \
 		hrt.cpp \
 		sbus2_test.cpp

-AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \
+ST24_FILES=../../src/lib/rc/st24.c \
+		hrt.cpp \
+		st24_test.cpp
+
+SF0X_FILES= \
+		hrt.cpp \
+		sf0x_test.cpp \
+		../../src/drivers/sf0x/sf0x_parser.cpp
+
+AUTODECLINATION_FILES= ../../src/lib/geo_lookup/geo_mag_declination.c \
 		hrt.cpp \
 		autodeclination_test.cpp

@@ -30,10 +39,16 @@ mixer_test: $(MIXER_FILES)
 sbus2_test: $(SBUS2_FILES)
 	$(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS)

+sf0x_test: $(SF0X_FILES)
+	$(CC) -o sf0x_test $(SF0X_FILES) $(CFLAGS)
+
 autodeclination_test: $(SBUS2_FILES)
 	$(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS)

+st24_test: $(ST24_FILES)
+	$(CC) -o st24_test $(ST24_FILES) $(CFLAGS)
+
 .PHONY: clean

 clean:
-	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test
+	rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test st24_test sf0x_test
@@ -29,7 +29,8 @@ int main(int argc, char *argv[]) {

 	// Trash the first 20 lines
 	for (unsigned i = 0; i < 20; i++) {
-		(void)fscanf(fp, "%f,%x,,", &f, &x);
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
 	}

 	// Init the parser
@@ -0,0 +1,65 @@
+
+#include <unistd.h>
+#include <string.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+
+#include <drivers/sf0x/sf0x_parser.h>
+
+int main(int argc, char *argv[])
+{
+	warnx("SF0X test started");
+
+	int ret = 0;
+
+	const char LINE_MAX = 20;
+	char _linebuf[LINE_MAX];
+	_linebuf[0] = '\0';
+
+	const char *lines[] = {"0.01\r\n",
+			       "0.02\r\n",
+			       "0.03\r\n",
+			       "0.04\r\n",
+			       "0",
+			       ".",
+			       "0",
+			       "5",
+			       "\r",
+			       "\n",
+			       "0",
+			       "3\r",
+			       "\n"
+			       "\r\n",
+			       "0.06",
+			       "\r\n"
+			      };
+
+	enum SF0X_PARSE_STATE state = SF0X_PARSE_STATE0_UNSYNC;
+	float dist_m;
+	char _parserbuf[LINE_MAX];
+	unsigned _parsebuf_index = 0;
+
+	for (unsigned l = 0; l < sizeof(lines) / sizeof(lines[0]); l++) {
+
+		printf("\n%s", _linebuf);
+
+		int parse_ret;
+
+		for (int i = 0; i < strlen(lines[l]); i++) {
+			parse_ret = sf0x_parser(lines[l][i], _parserbuf, &_parsebuf_index, &state, &dist_m);
+
+			if (parse_ret == 0) {
+				printf("\nparsed: %f %s\n", dist_m, (parse_ret == 0) ? "OK" : "");
+			}
+		}
+
+		printf("%s", lines[l]);
+
+	}
+
+	warnx("test finished");
+
+	return ret;
+}
@@ -0,0 +1,76 @@
+
+#include <stdio.h>
+#include <unistd.h>
+#include <string.h>
+#include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
+#include <rc/st24.h>
+#include "../../src/systemcmds/tests/tests.h"
+
+int main(int argc, char *argv[])
+{
+	warnx("ST24 test started");
+
+	if (argc < 2) {
+		errx(1, "Need a filename for the input file");
+	}
+
+	warnx("loading data from: %s", argv[1]);
+
+	FILE *fp;
+
+	fp = fopen(argv[1], "rt");
+
+	if (!fp) {
+		errx(1, "failed opening file");
+	}
+
+	float f;
+	unsigned x;
+	int ret;
+
+	// Trash the first 20 lines
+	for (unsigned i = 0; i < 20; i++) {
+		char buf[200];
+		(void)fgets(buf, sizeof(buf), fp);
+	}
+
+	float last_time = 0;
+
+	while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) {
+		if (((f - last_time) * 1000 * 1000) > 3000) {
+			// warnx("FRAME RESET\n\n");
+		}
+
+		uint8_t b = static_cast<uint8_t>(x);
+
+		last_time = f;
+
+		// Pipe the data into the parser
+		hrt_abstime now = hrt_absolute_time();
+
+		uint8_t rssi;
+		uint8_t rx_count;
+		uint16_t channel_count;
+		uint16_t channels[20];
+
+
+		if (!st24_decode(b, &rssi, &rx_count, &channel_count, channels, sizeof(channels) / sizeof(channels[0]))) {
+			warnx("decoded: %u channels (converted to PPM range)", (unsigned)channel_count);
+
+			for (unsigned i = 0; i < channel_count; i++) {
+
+				int16_t val = channels[i];
+				warnx("channel %u: %d 0x%03X", i, static_cast<int>(val), static_cast<int>(val));
+			}
+		}
+	}
+
+	if (ret == EOF) {
+		warnx("Test finished, reached end of file");
+
+	} else {
+		warnx("Test aborted, errno: %d", ret);
+	}
+
+}
@@ -128,6 +128,11 @@ MODULES		+= lib/geo_lookup
 MODULES		+= lib/conversion
 MODULES		+= lib/launchdetection

+#
+# OBC challenge
+#
+MODULES		+= modules/bottle_drop
+
 #
 # Demo apps
 #
@@ -56,6 +56,7 @@ LIBRARIES	+= lib/mathlib/CMSIS

 MODULES 	+= modules/unit_test
 MODULES		+= modules/mavlink/mavlink_tests
+MODULES 	+= modules/commander/commander_tests

 #
 # Transitional support - add commands from the NuttX export archive.
@@ -83,7 +83,8 @@ enum RC_INPUT_SOURCE {
 	RC_INPUT_SOURCE_PX4FMU_PPM,
 	RC_INPUT_SOURCE_PX4IO_PPM,
 	RC_INPUT_SOURCE_PX4IO_SPEKTRUM,
-	RC_INPUT_SOURCE_PX4IO_SBUS
+	RC_INPUT_SOURCE_PX4IO_SBUS,
+	RC_INPUT_SOURCE_PX4IO_ST24
 };

 /**
@@ -189,6 +189,18 @@ UBX::configure(unsigned &baudrate)
 		return 1;
 	}

+#ifdef UBX_CONFIGURE_SBAS
+	/* send a SBAS message to set the SBAS options */
+	memset(&_buf.payload_tx_cfg_sbas, 0, sizeof(_buf.payload_tx_cfg_sbas));
+	_buf.payload_tx_cfg_sbas.mode		= UBX_TX_CFG_SBAS_MODE;
+
+	send_message(UBX_MSG_CFG_SBAS, _buf.raw, sizeof(_buf.payload_tx_cfg_sbas));
+
+	if (wait_for_ack(UBX_MSG_CFG_SBAS, UBX_CONFIG_TIMEOUT, true) < 0) {
+		return 1;
+	}
+#endif
+
 	/* configure message rates */
 	/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */

@@ -73,6 +73,7 @@
 #define UBX_ID_CFG_MSG		0x01
 #define UBX_ID_CFG_RATE		0x08
 #define UBX_ID_CFG_NAV5		0x24
+#define UBX_ID_CFG_SBAS		0x16
 #define UBX_ID_MON_VER		0x04
 #define UBX_ID_MON_HW		0x09

@@ -89,6 +90,7 @@
 #define UBX_MSG_CFG_MSG		((UBX_CLASS_CFG) | UBX_ID_CFG_MSG << 8)
 #define UBX_MSG_CFG_RATE	((UBX_CLASS_CFG) | UBX_ID_CFG_RATE << 8)
 #define UBX_MSG_CFG_NAV5	((UBX_CLASS_CFG) | UBX_ID_CFG_NAV5 << 8)
+#define UBX_MSG_CFG_SBAS	((UBX_CLASS_CFG) | UBX_ID_CFG_SBAS << 8)
 #define UBX_MSG_MON_HW		((UBX_CLASS_MON) | UBX_ID_MON_HW << 8)
 #define UBX_MSG_MON_VER		((UBX_CLASS_MON) | UBX_ID_MON_VER << 8)

@@ -128,6 +130,11 @@
 #define UBX_TX_CFG_NAV5_DYNMODEL	7		/**< 0 Portable, 2 Stationary, 3 Pedestrian, 4 Automotive, 5 Sea, 6 Airborne <1g, 7 Airborne <2g, 8 Airborne <4g */
 #define UBX_TX_CFG_NAV5_FIXMODE		2		/**< 1 2D only, 2 3D only, 3 Auto 2D/3D */

+/* TX CFG-SBAS message contents */
+#define UBX_TX_CFG_SBAS_MODE_ENABLED	1				/**< SBAS enabled */
+#define UBX_TX_CFG_SBAS_MODE_DISABLED	0				/**< SBAS disabled */
+#define UBX_TX_CFG_SBAS_MODE		UBX_TX_CFG_SBAS_MODE_DISABLED	/**< SBAS enabled or disabled */
+
 /* TX CFG-MSG message contents */
 #define UBX_TX_CFG_MSG_RATE1_5HZ	0x01 		/**< {0x00, 0x01, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
 #define UBX_TX_CFG_MSG_RATE1_1HZ	0x05		/**< {0x00, 0x05, 0x00, 0x00, 0x00, 0x00} the second entry is for UART1 */
@@ -383,6 +390,15 @@ typedef struct {
 	uint32_t	reserved4;
 } ubx_payload_tx_cfg_nav5_t;

+/* tx cfg-sbas */
+typedef struct {
+	uint8_t		mode;
+	uint8_t		usage;
+	uint8_t		maxSBAS;
+	uint8_t		scanmode2;
+	uint32_t	scanmode1;
+} ubx_payload_tx_cfg_sbas_t;
+
 /* Tx CFG-MSG */
 typedef struct {
 	union {
@@ -413,6 +429,7 @@ typedef union {
 	ubx_payload_tx_cfg_prt_t		payload_tx_cfg_prt;
 	ubx_payload_tx_cfg_rate_t		payload_tx_cfg_rate;
 	ubx_payload_tx_cfg_nav5_t		payload_tx_cfg_nav5;
+	ubx_payload_tx_cfg_sbas_t		payload_tx_cfg_sbas;
 	ubx_payload_tx_cfg_msg_t		payload_tx_cfg_msg;
 	uint8_t					raw[];
 } ubx_buf_t;
@@ -392,7 +392,8 @@ HIL::task_main()
 		if (fds[0].revents & POLLIN) {

 			/* get controls - must always do this to avoid spinning */
-			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, _t_actuators, &_controls);
+			orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS :
+				     ORB_ID(actuator_controls_1), _t_actuators, &_controls);

 			/* can we mix? */
 			if (_mixers != nullptr) {
@@ -1617,6 +1617,9 @@ PX4IO::io_publish_raw_rc()
 	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
 		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_SBUS;

+	} else if (_status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
+		rc_val.input_source = RC_INPUT_SOURCE_PX4IO_ST24;
+
 	} else {
 		rc_val.input_source = RC_INPUT_SOURCE_UNKNOWN;

@@ -1934,13 +1937,15 @@ PX4IO::print_status(bool extended_status)
 	       io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
 	uint16_t io_status_flags = flags;
-	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
+	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
 	       flags,
 	       ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
 	       ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_RC_DSM)   ? " DSM" : ""),
+	       ((flags & PX4IO_P_STATUS_FLAGS_RC_ST24)   ? " ST24" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
 	       ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
 	       ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PWM_PASSTHROUGH" : ""),
@@ -2465,6 +2470,9 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 			} else if (status & PX4IO_P_STATUS_FLAGS_RC_SBUS) {
 				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_SBUS;

+			} else if (status & PX4IO_P_STATUS_FLAGS_RC_ST24) {
+				rc_val->input_source = RC_INPUT_SOURCE_PX4IO_ST24;
+
 			} else {
 				rc_val->input_source = RC_INPUT_SOURCE_UNKNOWN;
 			}
@@ -37,6 +37,7 @@

 MODULE_COMMAND	= sf0x

-SRCS		= sf0x.cpp
+SRCS		= sf0x.cpp \
+		  sf0x_parser.cpp

 MAXOPTIMIZATION	 = -Os
@@ -72,6 +72,8 @@

 #include <board_config.h>

+#include "sf0x_parser.h"
+
 /* Configuration Constants */

 /* oddly, ERROR is not defined for c++ */
@@ -120,6 +122,7 @@ private:
 	int				_fd;
 	char				_linebuf[10];
 	unsigned			_linebuf_index;
+	enum SF0X_PARSE_STATE		_parse_state;
 	hrt_abstime			_last_read;

 	orb_advert_t			_range_finder_topic;
@@ -186,6 +189,7 @@ SF0X::SF0X(const char *port) :
 	_collect_phase(false),
 	_fd(-1),
 	_linebuf_index(0),
+	_parse_state(SF0X_PARSE_STATE0_UNSYNC),
 	_last_read(0),
 	_range_finder_topic(-1),
 	_consecutive_fail_count(0),
@@ -200,12 +204,6 @@ SF0X::SF0X(const char *port) :
 		warnx("FAIL: laser fd");
 	}

-	/* tell it to stop auto-triggering */
-	char stop_auto = ' ';
-	(void)::write(_fd, &stop_auto, 1);
-	usleep(100);
-	(void)::write(_fd, &stop_auto, 1);
-
 	struct termios uart_config;

 	int termios_state;
@@ -520,20 +518,15 @@ SF0X::collect()
 	/* clear buffer if last read was too long ago */
 	uint64_t read_elapsed = hrt_elapsed_time(&_last_read);

-	/* timed out - retry */
-	if (read_elapsed > (SF0X_CONVERSION_INTERVAL * 2)) {
-		_linebuf_index = 0;
-	}
-
 	/* the buffer for read chars is buflen minus null termination */
-	unsigned readlen = sizeof(_linebuf) - 1;
+	char readbuf[sizeof(_linebuf)];
+	unsigned readlen = sizeof(readbuf) - 1;

 	/* read from the sensor (uart buffer) */
-	ret = ::read(_fd, &_linebuf[_linebuf_index], readlen - _linebuf_index);
+	ret = ::read(_fd, &readbuf[0], readlen);

 	if (ret < 0) {
-		_linebuf[sizeof(_linebuf) - 1] = '\0';
-		debug("read err: %d lbi: %d buf: %s", ret, (int)_linebuf_index, _linebuf);
+		debug("read err: %d", ret);
 		perf_count(_comms_errors);
 		perf_end(_sample_perf);

@@ -548,84 +541,23 @@ SF0X::collect()
 		return -EAGAIN;
 	}

-	/* let the write pointer point to the next free entry */
-	_linebuf_index += ret;
-
 	_last_read = hrt_absolute_time();

-	/* require a reasonable amount of minimum bytes */
-	if (_linebuf_index < 6) {
-		/* we need at this format: x.xx\r\n */
-		return -EAGAIN;
-
-	} else if (_linebuf[_linebuf_index - 2] != '\r' || _linebuf[_linebuf_index - 1] != '\n') {
-
-		if (_linebuf_index == readlen) {
-			/* we have a full buffer, but no line ending - abort */
-			_linebuf_index = 0;
-			perf_count(_comms_errors);
-			return -ENOMEM;
-		} else {
-			/* incomplete read, reschedule ourselves */
-			return -EAGAIN;
+	float si_units;
+	bool valid = false;
+	
+	for (unsigned i = 0; i < ret; i++) {
+		if (OK == sf0x_parser(readbuf[i], _linebuf, &_linebuf_index, &_parse_state, &si_units)) {
+			valid = true;
 		}
 	}

-	char *end;
-	float si_units;
-	bool valid;
-
-	/* enforce line ending */
-	_linebuf[_linebuf_index] = '\0';
-
-	if (_linebuf[0] == '-' && _linebuf[1] == '-' && _linebuf[2] == '.') {
-		si_units = -1.0f;
-		valid = false;
-
-	} else {
-
-		/* we need to find a dot in the string, as we're missing the meters part else */
-		valid = false;
-
-		/* wipe out partially read content from last cycle(s), check for dot */
-		for (unsigned i = 0; i < (_linebuf_index - 2); i++) {
-			if (_linebuf[i] == '\n') {
-				/* wipe out any partial measurements */
-				for (unsigned j = 0; j <= i; j++) {
-					_linebuf[j] = ' ';
-				}
-			}
-
-			/* we need a digit before the dot and a dot for a valid number */
-			if (i > 0 && ((_linebuf[i - 1] >= '0') && (_linebuf[i - 1] <= '9')) && (_linebuf[i] == '.')) {
-				valid = true;
-			}
-		}
-
-		if (valid) {
-			si_units = strtod(_linebuf, &end);
-
-			/* we require at least four characters for a valid number */
-			if (end > _linebuf + 3) {
-				valid = true;
-			} else {
-				si_units = -1.0f;
-				valid = false;
-			}
-		}
+	if (!valid) {
+		return -EAGAIN;
 	}

 	debug("val (float): %8.4f, raw: %s, valid: %s", (double)si_units, _linebuf, ((valid) ? "OK" : "NO"));

-	/* done with this chunk, resetting - even if invalid */
-	_linebuf_index = 0;
-
-	/* if its invalid, there is no reason to forward the value */
-	if (!valid) {
-		perf_count(_comms_errors);
-		return -EINVAL;
-	}
-
 	struct range_finder_report report;

 	/* this should be fairly close to the end of the measurement, so the best approximation of the time */
@@ -0,0 +1,155 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sf0x_parser.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * Driver for the Lightware SF0x laser rangefinder series
+ */
+
+#include "sf0x_parser.h"
+#include <string.h>
+#include <stdlib.h>
+
+//#define SF0X_DEBUG
+
+#ifdef SF0X_DEBUG
+#include <stdio.h>
+
+const char *parser_state[] = {
+	"0_UNSYNC",
+	"1_SYNC",
+	"2_GOT_DIGIT0",
+	"3_GOT_DOT",
+	"4_GOT_DIGIT1",
+	"5_GOT_DIGIT2",
+	"6_GOT_CARRIAGE_RETURN"
+};
+#endif
+
+int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist)
+{
+	int ret = -1;
+	char *end;
+
+	switch (*state) {
+	case SF0X_PARSE_STATE0_UNSYNC:
+		if (c == '\n') {
+			*state = SF0X_PARSE_STATE1_SYNC;
+			(*parserbuf_index) = 0;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE1_SYNC:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE2_GOT_DIGIT0:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE2_GOT_DIGIT0;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else if (c == '.') {
+			*state = SF0X_PARSE_STATE3_GOT_DOT;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE3_GOT_DOT:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE4_GOT_DIGIT1;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE4_GOT_DIGIT1:
+		if (c >= '0' && c <= '9') {
+			*state = SF0X_PARSE_STATE5_GOT_DIGIT2;
+			parserbuf[*parserbuf_index] = c;
+			(*parserbuf_index)++;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE5_GOT_DIGIT2:
+		if (c == '\r') {
+			*state = SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+
+	case SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN:
+		if (c == '\n') {
+			parserbuf[*parserbuf_index] = '\0';
+			*dist = strtod(parserbuf, &end);
+			*state = SF0X_PARSE_STATE1_SYNC;
+			*parserbuf_index = 0;
+			ret = 0;
+
+		} else {
+			*state = SF0X_PARSE_STATE0_UNSYNC;
+		}
+
+		break;
+	}
+
+#ifdef SF0X_DEBUG
+	printf("state: SF0X_PARSE_STATE%s\n", parser_state[*state]);
+#endif
+
+	return ret;
+}
@@ -1,4 +1,4 @@
-/***************************************************************************
+/****************************************************************************
 *
 *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
 *
@@ -30,43 +30,22 @@
 * POSSIBILITY OF SUCH DAMAGE.
 *
 ****************************************************************************/
+
 /**
- * @file offboard.h
+ * @file sf0x_parser.cpp
+ * @author Lorenz Meier <lm@inf.ethz.ch>
 *
- * Helper class for offboard commands
- *
- * @author Julian Oes <julian@oes.ch>
+ * Declarations of parser for the Lightware SF0x laser rangefinder series
 */

-#ifndef NAVIGATOR_OFFBOARD_H
-#define NAVIGATOR_OFFBOARD_H
-
-#include <controllib/blocks.hpp>
-#include <controllib/block/BlockParam.hpp>
-
-#include <uORB/uORB.h>
-#include <uORB/topics/offboard_control_setpoint.h>
-
-#include "navigator_mode.h"
-
-class Navigator;
-
-class Offboard : public NavigatorMode
-{
-public:
-	Offboard(Navigator *navigator, const char *name);
-
-	~Offboard();
-
-	virtual void on_inactive();
-
-	virtual void on_activation();
-
-	virtual void on_active();
-private:
-	void update_offboard_control_setpoint();
-
-	struct offboard_control_setpoint_s _offboard_control_sp;
+enum SF0X_PARSE_STATE {
+	SF0X_PARSE_STATE0_UNSYNC = 0,
+	SF0X_PARSE_STATE1_SYNC,
+	SF0X_PARSE_STATE2_GOT_DIGIT0,
+	SF0X_PARSE_STATE3_GOT_DOT,
+	SF0X_PARSE_STATE4_GOT_DIGIT1,
+	SF0X_PARSE_STATE5_GOT_DIGIT2,
+	SF0X_PARSE_STATE6_GOT_CARRIAGE_RETURN
 };

-#endif
+int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist);
@@ -236,9 +236,9 @@ void TECS::_update_height_demand(float demand, float state)
 //	// 	_hgt_rate_dem);

 	_hgt_dem_adj = demand;//0.025f * demand + 0.975f * _hgt_dem_adj_last;
+	_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
 	_hgt_dem_adj_last = _hgt_dem_adj;

-	_hgt_rate_dem = (_hgt_dem_adj-state)*_heightrate_p + _heightrate_ff * (_hgt_dem_adj - _hgt_dem_adj_last)/_DT;
 	// Limit height rate of change
 	if (_hgt_rate_dem > _maxClimbRate) {
 		_hgt_rate_dem = _maxClimbRate;
@@ -0,0 +1,40 @@
+############################################################################
+#
+#   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Yuntec ST24 transmitter protocol decoder
+#
+
+SRCS		 =	st24.c
+
+MAXOPTIMIZATION	 = -Os
@@ -0,0 +1,253 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/*
+ * @file st24.h
+ *
+ * RC protocol implementation for Yuneec ST24 transmitter.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#include <stdbool.h>
+#include <stdio.h>
+#include "st24.h"
+
+enum ST24_DECODE_STATE {
+	ST24_DECODE_STATE_UNSYNCED = 0,
+	ST24_DECODE_STATE_GOT_STX1,
+	ST24_DECODE_STATE_GOT_STX2,
+	ST24_DECODE_STATE_GOT_LEN,
+	ST24_DECODE_STATE_GOT_TYPE,
+	ST24_DECODE_STATE_GOT_DATA
+};
+
+const char *decode_states[] = {"UNSYNCED",
+			       "GOT_STX1",
+			       "GOT_STX2",
+			       "GOT_LEN",
+			       "GOT_TYPE",
+			       "GOT_DATA"
+			      };
+
+/* define range mapping here, -+100% -> 1000..2000 */
+#define ST24_RANGE_MIN 0.0f
+#define ST24_RANGE_MAX 4096.0f
+
+#define ST24_TARGET_MIN 1000.0f
+#define ST24_TARGET_MAX 2000.0f
+
+/* pre-calculate the floating point stuff as far as possible at compile time */
+#define ST24_SCALE_FACTOR ((ST24_TARGET_MAX - ST24_TARGET_MIN) / (ST24_RANGE_MAX - ST24_RANGE_MIN))
+#define ST24_SCALE_OFFSET (int)(ST24_TARGET_MIN - (ST24_SCALE_FACTOR * ST24_RANGE_MIN + 0.5f))
+
+static enum ST24_DECODE_STATE _decode_state = ST24_DECODE_STATE_UNSYNCED;
+static unsigned _rxlen;
+
+static ReceiverFcPacket _rxpacket;
+
+uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len)
+{
+	uint8_t i, crc ;
+	crc = 0;
+
+	while (len--) {
+		for (i = 0x80; i != 0; i >>= 1) {
+			if ((crc & 0x80) != 0) {
+				crc <<= 1;
+				crc ^= 0x07;
+
+			} else {
+				crc <<= 1;
+			}
+
+			if ((*ptr & i) != 0) {
+				crc ^= 0x07;
+			}
+		}
+
+		ptr++;
+	}
+
+	return (crc);
+}
+
+
+int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count, uint16_t *channels,
+		    uint16_t max_chan_count)
+{
+
+	int ret = 1;
+
+	switch (_decode_state) {
+	case ST24_DECODE_STATE_UNSYNCED:
+		if (byte == ST24_STX1) {
+			_decode_state = ST24_DECODE_STATE_GOT_STX1;
+		} else {
+			ret = 3;
+		}
+
+		break;
+
+	case ST24_DECODE_STATE_GOT_STX1:
+		if (byte == ST24_STX2) {
+			_decode_state = ST24_DECODE_STATE_GOT_STX2;
+
+		} else {
+			_decode_state = ST24_DECODE_STATE_UNSYNCED;
+		}
+
+		break;
+
+	case ST24_DECODE_STATE_GOT_STX2:
+
+		/* ensure no data overflow failure or hack is possible */
+		if ((unsigned)byte <= sizeof(_rxpacket.length) + sizeof(_rxpacket.type) + sizeof(_rxpacket.st24_data)) {
+			_rxpacket.length = byte;
+			_rxlen = 0;
+			_decode_state = ST24_DECODE_STATE_GOT_LEN;
+
+		} else {
+			_decode_state = ST24_DECODE_STATE_UNSYNCED;
+		}
+
+		break;
+
+	case ST24_DECODE_STATE_GOT_LEN:
+		_rxpacket.type = byte;
+		_rxlen++;
+		_decode_state = ST24_DECODE_STATE_GOT_TYPE;
+		break;
+
+	case ST24_DECODE_STATE_GOT_TYPE:
+		_rxpacket.st24_data[_rxlen - 1] = byte;
+		_rxlen++;
+
+		if (_rxlen == (_rxpacket.length - 1)) {
+			_decode_state = ST24_DECODE_STATE_GOT_DATA;
+		}
+
+		break;
+
+	case ST24_DECODE_STATE_GOT_DATA:
+		_rxpacket.crc8 = byte;
+		_rxlen++;
+
+		if (st24_common_crc8((uint8_t *) & (_rxpacket.length), _rxlen) == _rxpacket.crc8) {
+
+			ret = 0;
+
+			/* decode the actual packet */
+
+			switch (_rxpacket.type) {
+
+			case ST24_PACKET_TYPE_CHANNELDATA12: {
+					ChannelData12 *d = (ChannelData12 *)_rxpacket.st24_data;
+
+					*rssi = d->rssi;
+					*rx_count = d->packet_count;
+
+					/* this can lead to rounding of the strides */
+					*channel_count = (max_chan_count < 12) ? max_chan_count : 12;
+
+					unsigned stride_count = (*channel_count * 3) / 2;
+					unsigned chan_index = 0;
+
+					for (unsigned i = 0; i < stride_count; i += 3) {
+						channels[chan_index] = ((uint16_t)d->channel[i] << 4);
+						channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
+						/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+						channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+						chan_index++;
+
+						channels[chan_index] = ((uint16_t)d->channel[i + 2]);
+						channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
+						/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+						channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+						chan_index++;
+					}
+				}
+				break;
+
+			case ST24_PACKET_TYPE_CHANNELDATA24: {
+					ChannelData24 *d = (ChannelData24 *)&_rxpacket.st24_data;
+
+					*rssi = d->rssi;
+					*rx_count = d->packet_count;
+
+					/* this can lead to rounding of the strides */
+					*channel_count = (max_chan_count < 24) ? max_chan_count : 24;
+
+					unsigned stride_count = (*channel_count * 3) / 2;
+					unsigned chan_index = 0;
+
+					for (unsigned i = 0; i < stride_count; i += 3) {
+						channels[chan_index] = ((uint16_t)d->channel[i] << 4);
+						channels[chan_index] |= ((uint16_t)(0xF0 & d->channel[i + 1]) >> 4);
+						/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+						channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+						chan_index++;
+
+						channels[chan_index] = ((uint16_t)d->channel[i + 2]);
+						channels[chan_index] |= (((uint16_t)(0x0F & d->channel[i + 1])) << 8);
+						/* convert values to 1000-2000 ppm encoding in a not too sloppy fashion */
+						channels[chan_index] = (uint16_t)(channels[chan_index] * ST24_SCALE_FACTOR + .5f) + ST24_SCALE_OFFSET;
+						chan_index++;
+					}
+				}
+				break;
+
+			case ST24_PACKET_TYPE_TRANSMITTERGPSDATA: {
+
+					// ReceiverFcPacket* d = (ReceiverFcPacket*)&_rxpacket.st24_data;
+					/* we silently ignore this data for now, as it is unused */
+					ret = 2;
+				}
+				break;
+
+			default:
+				ret = 2;
+				break;
+			}
+
+		} else {
+			/* decoding failed */
+			ret = 4;
+		}
+
+		_decode_state = ST24_DECODE_STATE_UNSYNCED;
+		break;
+	}
+
+	return ret;
+}
@@ -0,0 +1,163 @@
+/****************************************************************************
+ *
+ *	Copyright (c) 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *	notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *	notice, this list of conditions and the following disclaimer in
+ *	the documentation and/or other materials provided with the
+ *	distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *	used to endorse or promote products derived from this software
+ *	without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file st24.h
+ *
+ * RC protocol definition for Yuneec ST24 transmitter
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ */
+
+#pragma once
+
+#include <stdint.h>
+
+__BEGIN_DECLS
+
+#define ST24_DATA_LEN_MAX	64
+#define ST24_STX1		0x55
+#define ST24_STX2		0x55
+
+enum ST24_PACKET_TYPE {
+	ST24_PACKET_TYPE_CHANNELDATA12 = 0,
+	ST24_PACKET_TYPE_CHANNELDATA24,
+	ST24_PACKET_TYPE_TRANSMITTERGPSDATA
+};
+
+#pragma pack(push, 1)
+typedef struct {
+	uint8_t	header1;			///< 0x55 for a valid packet
+	uint8_t	header2;			///< 0x55 for a valid packet
+	uint8_t	length;				///< length includes type, data, and crc = sizeof(type)+sizeof(data[payload_len])+sizeof(crc8)
+	uint8_t	type;				///< from enum ST24_PACKET_TYPE
+	uint8_t	st24_data[ST24_DATA_LEN_MAX];
+	uint8_t	crc8;				///< crc8 checksum, calculated by st24_common_crc8 and including fields length, type and st24_data
+} ReceiverFcPacket;
+
+/**
+ * RC Channel data (12 channels).
+ *
+ * This is incoming from the ST24
+ */
+typedef struct {
+	uint16_t t;			///< packet counter or clock
+	uint8_t	rssi;			///< signal strength
+	uint8_t	packet_count;		///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
+	uint8_t	channel[18];		///< channel data, 12 channels (12 bit numbers)
+} ChannelData12;
+
+/**
+ * RC Channel data (12 channels).
+ *
+ */
+typedef struct {
+	uint16_t t;			///< packet counter or clock
+	uint8_t	rssi;			///< signal strength
+	uint8_t	packet_count;		///< Number of UART packets sent since reception of last RF frame (this tells something about age / rate)
+	uint8_t	channel[36];		///< channel data, 24 channels (12 bit numbers)
+} ChannelData24;
+
+/**
+ * Telemetry packet
+ *
+ * This is outgoing to the ST24
+ *
+ * imuStatus:
+ * 8 bit total
+ * bits 0-2 for status
+ * - value 0 is FAILED
+ * - value 1 is INITIALIZING
+ * - value 2 is RUNNING
+ * - values 3 through 7 are reserved
+ * bits 3-7 are status for sensors (0 or 1)
+ * - mpu6050
+ * - accelerometer
+ * - primary gyro x
+ * - primary gyro y
+ * - primary gyro z
+ *
+ * pressCompassStatus
+ * 8 bit total
+ * bits 0-3 for compass status
+ * - value 0 is FAILED
+ * - value 1 is INITIALIZING
+ * - value 2 is RUNNING
+ * - value 3 - 15 are reserved
+ * bits 4-7 for pressure status
+ * - value 0 is FAILED
+ * - value 1 is INITIALIZING
+ * - value 2 is RUNNING
+ * - value 3 - 15 are reserved
+ *
+ */
+typedef struct {
+	uint16_t t;			///< packet counter or clock
+	int32_t	lat;			///< lattitude (degrees)	+/- 90 deg
+	int32_t	lon;			///< longitude (degrees)	+/- 180 deg
+	int32_t	alt;			///< 0.01m resolution, altitude (meters)
+	int16_t	vx, vy, vz; 		///< velocity 0.01m res, +/-320.00 North-East- Down
+	uint8_t	nsat;			///<number of satellites
+	uint8_t	voltage; 		///< 25.4V	voltage = 5 + 255*0.1 = 30.5V, min=5V
+	uint8_t	current; 		///< 0.5A resolution
+	int16_t	roll, pitch, yaw;	///< 0.01 degree resolution
+	uint8_t	motorStatus;		///< 1 bit per motor for status 1=good, 0= fail
+	uint8_t	imuStatus;		///< inertial measurement unit status
+	uint8_t	pressCompassStatus;	///< baro / compass status
+} TelemetryData;
+
+#pragma pack(pop)
+
+/**
+ * CRC8 implementation for ST24 protocol
+ *
+ * @param prt Pointer to the data to CRC
+ * @param len number of bytes to accumulate in the checksum
+ * @return the checksum of these bytes over len
+ */
+uint8_t st24_common_crc8(uint8_t *ptr, uint8_t len);
+
+/**
+ * Decoder for ST24 protocol
+ *
+ * @param byte current char to read
+ * @param rssi pointer to a byte where the RSSI value is written back to
+ * @param rx_count pointer to a byte where the receive count of packets signce last wireless frame is written back to
+ * @param channels pointer to a datastructure of size max_chan_count where channel values (12 bit) are written back to
+ * @param max_chan_count maximum channels to decode - if more channels are decoded, the last n are skipped and success (0) is returned
+ * @return 0 for success (a decoded packet), 1 for no packet yet (accumulating), 2 for unknown packet, 3 for out of sync, 4 for checksum error
+ */
+__EXPORT int st24_decode(uint8_t byte, uint8_t *rssi, uint8_t *rx_count, uint16_t *channel_count,
+			     uint16_t *channels, uint16_t max_chan_count);
+
+__END_DECLS
@@ -0,0 +1,131 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file bottle_drop_params.c
+ * Bottle drop parameters
+ *
+ * @author Dominik Juchli <juchlid@ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <systemlib/param/param.h>
+
+/**
+ * Ground drag property
+ *
+ * This parameter encodes the ground drag coefficient and the corresponding
+ * decrease in wind speed from the plane altitude to ground altitude.
+ *
+ * @unit unknown
+ * @min 0.001
+ * @max 0.1
+ * @group Payload drop
+ */
+PARAM_DEFINE_FLOAT(BD_GPROPERTIES, 0.03f);
+
+/**
+ * Plane turn radius
+ *
+ * The planes known minimal turn radius - use a higher value
+ * to make the plane maneuver more distant from the actual drop
+ * position. This is to ensure the wings are level during the drop.
+ *
+ * @unit meter
+ * @min 30.0
+ * @max 500.0
+ * @group Payload drop
+ */
+PARAM_DEFINE_FLOAT(BD_TURNRADIUS, 120.0f);
+
+/**
+ * Drop precision
+ *
+ * If the system is closer than this distance on passing over the
+ * drop position, it will release the payload. This is a safeguard
+ * to prevent a drop out of the required accuracy.
+ *
+ * @unit meter
+ * @min 1.0
+ * @max 80.0
+ * @group Payload drop
+ */
+PARAM_DEFINE_FLOAT(BD_PRECISION, 30.0f);
+
+/**
+ * Payload drag coefficient of the dropped object
+ *
+ * The drag coefficient (cd) is the typical drag
+ * constant for air. It is in general object specific,
+ * but the closest primitive shape to the actual object
+ * should give good results:
+ * http://en.wikipedia.org/wiki/Drag_coefficient
+ *
+ * @unit meter
+ * @min 0.08
+ * @max 1.5
+ * @group Payload drop
+ */
+PARAM_DEFINE_FLOAT(BD_OBJ_CD, 0.1f);
+
+/**
+ * Payload mass
+ *
+ * A typical small toy ball:
+ *   0.025 kg
+ *
+ * OBC water bottle:
+ *   0.6 kg
+ *
+ * @unit kilogram
+ * @min 0.001
+ * @max 5.0
+ * @group Payload drop
+ */
+PARAM_DEFINE_FLOAT(BD_OBJ_MASS, 0.6f);
+
+/**
+ * Payload front surface area
+ *
+ * A typical small toy ball:
+ *   (0.045 * 0.045) / 4.0 * pi = 0.001590 m^2
+ *
+ * OBC water bottle:
+ *   (0.063 * 0.063) / 4.0 * pi = 0.003117 m^2
+ *
+ * @unit m^2
+ * @min 0.001
+ * @max 0.5
+ * @group Payload drop
+ */
+PARAM_DEFINE_FLOAT(BD_OBJ_SURFACE, 0.00311724531f);
@@ -0,0 +1,41 @@
+############################################################################
+#
+#   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# Daemon application
+#
+
+MODULE_COMMAND		= bottle_drop
+
+SRCS			= bottle_drop.cpp \
+			  bottle_drop_params.c
@@ -633,7 +633,29 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			}
 		}
 		break;
-
+	case VEHICLE_CMD_NAV_GUIDED_ENABLE: {
+			transition_result_t res = TRANSITION_DENIED;
+			static main_state_t main_state_pre_offboard = MAIN_STATE_MANUAL;
+			if (status_local->main_state != MAIN_STATE_OFFBOARD) {
+				main_state_pre_offboard = status_local->main_state;
+			}
+			if (cmd->param1 > 0.5f) {
+				res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+				if (res == TRANSITION_DENIED) {
+					print_reject_mode(status_local, "OFFBOARD");
+					status_local->offboard_control_set_by_command = false;
+				} else {
+					/* Set flag that offboard was set via command, main state is not overridden by rc */
+					status_local->offboard_control_set_by_command = true;
+				}
+			} else {
+				/* If the mavlink command is used to enable or disable offboard control:
+				 * switch back to previous mode when disabling */
+				res = main_state_transition(status_local, main_state_pre_offboard);
+				status_local->offboard_control_set_by_command = false;
+			}
+		}
+		break;
 	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
 	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -1958,6 +1980,11 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 	/* set main state according to RC switches */
 	transition_result_t res = TRANSITION_DENIED;

+	/* if offboard is set allready by a mavlink command, abort */
+	if (status.offboard_control_set_by_command) {
+		return main_state_transition(status_local, MAIN_STATE_OFFBOARD);
+	}
+
 	/* offboard switch overrides main switch */
 	if (sp_man->offboard_switch == SWITCH_POS_ON) {
 		res = main_state_transition(status_local, MAIN_STATE_OFFBOARD);
@@ -2150,21 +2177,26 @@ set_control_mode()
 			control_mode.flag_control_position_enabled = false;
 			control_mode.flag_control_velocity_enabled = false;
 			break;
-		case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY:
+		case OFFBOARD_CONTROL_MODE_DIRECT_FORCE:
 			control_mode.flag_control_rates_enabled = true;
-			control_mode.flag_control_attitude_enabled = true;
-			control_mode.flag_control_altitude_enabled = true; /* XXX: hack for now */
-			control_mode.flag_control_climb_rate_enabled = true;
-			control_mode.flag_control_position_enabled = true; /* XXX: hack for now */
-			control_mode.flag_control_velocity_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_force_enabled = true;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
 			break;
-		case OFFBOARD_CONTROL_MODE_DIRECT_POSITION:
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED:
+		case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED:
 			control_mode.flag_control_rates_enabled = true;
 			control_mode.flag_control_attitude_enabled = true;
 			control_mode.flag_control_altitude_enabled = true;
 			control_mode.flag_control_climb_rate_enabled = true;
 			control_mode.flag_control_position_enabled = true;
 			control_mode.flag_control_velocity_enabled = true;
+			//XXX: the flags could depend on sp_offboard.ignore
 			break;
 		default:
 			control_mode.flag_control_rates_enabled = false;
@@ -48,7 +48,5 @@ extern "C" __EXPORT int commander_tests_main(int argc, char *argv[]);

 int commander_tests_main(int argc, char *argv[])
 {
-	stateMachineHelperTest();
-
-	return 0;
+	return stateMachineHelperTest() ? 0 : -1;
 }
@@ -49,7 +49,7 @@ public:
 	StateMachineHelperTest();
 	virtual ~StateMachineHelperTest();

-	virtual void runTests(void);
+	virtual bool run_tests(void);

 private:
 	bool armingStateTransitionTest();
@@ -488,16 +488,13 @@ bool StateMachineHelperTest::isSafeTest(void)
 	return true;
 }

-void StateMachineHelperTest::runTests(void)
+bool StateMachineHelperTest::run_tests(void)
 {
 	ut_run_test(armingStateTransitionTest);
 	ut_run_test(mainStateTransitionTest);
 	ut_run_test(isSafeTest);
+	
+	return (_tests_failed == 0);
 }

-void stateMachineHelperTest(void)
-{
-	StateMachineHelperTest* test = new StateMachineHelperTest();
-    test->runTests();
-	test->printResults();
-}
+ut_declare_test(stateMachineHelperTest, StateMachineHelperTest)
@@ -39,6 +39,6 @@
 #ifndef STATE_MACHINE_HELPER_TEST_H_
 #define STATE_MACHINE_HELPER_TEST_

-void stateMachineHelperTest(void);
+bool stateMachineHelperTest(void);

 #endif /* STATE_MACHINE_HELPER_TEST_H_ */
@@ -40,3 +40,5 @@ MODULE_COMMAND	= dataman
 SRCS		= dataman.c

 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION = -Os
@@ -1369,7 +1369,7 @@ FixedwingEstimator::task_main()
 					if (newRangeData) {
 						_ekf->fuseRngData = true;
 						_ekf->useRangeFinder = true;
-						_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 500.0f));
+						_ekf->RecallStates(_ekf->statesAtRngTime, (IMUmsec - 100.0f));
 						_ekf->GroundEKF();
 					}

@@ -88,7 +88,6 @@
 #include <launchdetection/LaunchDetector.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <external_lgpl/tecs/tecs.h>
-#include <drivers/drv_range_finder.h>
 #include "landingslope.h"
 #include "mtecs/mTecs.h"

@@ -146,7 +145,6 @@ private:
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
 	int		_sensor_combined_sub;		/**< for body frame accelerations */
-	int		_range_finder_sub;		/**< range finder subscription */

 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
 	orb_advert_t	_tecs_status_pub;		/**< TECS status publication */
@@ -162,17 +160,16 @@ private:
 	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
 	struct position_setpoint_triplet_s		_pos_sp_triplet;		/**< triplet of mission items */
 	struct sensor_combined_s			_sensor_combined;		/**< for body frame accelerations */
-	struct range_finder_report 			_range_finder;			/**< range finder report */

 	perf_counter_t	_loop_perf;			/**< loop performance counter */

 	/* land states */
-	/* not in non-abort mode for landing yet */
 	bool land_noreturn_horizontal;
 	bool land_noreturn_vertical;
 	bool land_stayonground;
 	bool land_motor_lim;
 	bool land_onslope;
+	bool land_useterrain;

 	/* takeoff/launch states */
 	LaunchDetectionResult launch_detection_state;
@@ -243,7 +240,9 @@ private:
 		float land_flare_alt_relative;
 		float land_thrust_lim_alt_relative;
 		float land_heading_hold_horizontal_distance;
-		float range_finder_rel_alt;
+		float land_flare_pitch_min_deg;
+		float land_flare_pitch_max_deg;
+		int land_use_terrain_estimate;

 	}		_parameters;			/**< local copies of interesting parameters */

@@ -289,7 +288,9 @@ private:
 		param_t land_flare_alt_relative;
 		param_t land_thrust_lim_alt_relative;
 		param_t land_heading_hold_horizontal_distance;
-		param_t range_finder_rel_alt;
+		param_t land_flare_pitch_min_deg;
+		param_t land_flare_pitch_max_deg;
+		param_t land_use_terrain_estimate;

 	}		_parameter_handles;		/**< handles for interesting parameters */

@@ -320,12 +321,6 @@ private:
 	 */
 	bool		vehicle_airspeed_poll();

-	/**
-	 * Check for range finder updates.
-	 */
-	bool		range_finder_poll();
-
-
 	/**
 	 * Check for position updates.
 	 */
@@ -347,9 +342,9 @@ private:
 	void navigation_capabilities_publish();

 	/**
-	 * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder
+	 * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
 	 */
-	float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt);
+	float get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos);

 	/**
 	 * Control position.
@@ -423,7 +418,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_params_sub(-1),
 	_manual_control_sub(-1),
 	_sensor_combined_sub(-1),
-	_range_finder_sub(-1),

 /* publications */
 	_attitude_sp_pub(-1),
@@ -441,7 +435,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_global_pos(),
 	_pos_sp_triplet(),
 	_sensor_combined(),
-	_range_finder(),

 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
@@ -451,6 +444,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_stayonground(false),
 	land_motor_lim(false),
 	land_onslope(false),
+	land_useterrain(false),
 	launch_detection_state(LAUNCHDETECTION_RES_NONE),
 	last_manual(false),
 	landingslope(),
@@ -489,7 +483,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
 	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
 	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
-	_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
+	_parameter_handles.land_flare_pitch_min_deg = param_find("FW_FLARE_PMIN");
+	_parameter_handles.land_flare_pitch_max_deg = param_find("FW_FLARE_PMAX");
+	_parameter_handles.land_use_terrain_estimate= param_find("FW_LND_USETER");

 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.time_const_throt = 			param_find("FW_T_THRO_CONST");
@@ -590,8 +586,9 @@ FixedwingPositionControl::parameters_update()
 	}

 	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));
-
-	param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
+	param_get(_parameter_handles.land_flare_pitch_min_deg, &(_parameters.land_flare_pitch_min_deg));
+	param_get(_parameter_handles.land_flare_pitch_max_deg, &(_parameters.land_flare_pitch_max_deg));
+	param_get(_parameter_handles.land_use_terrain_estimate, &(_parameters.land_use_terrain_estimate));

 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
@@ -695,20 +692,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 	return airspeed_updated;
 }

-bool
-FixedwingPositionControl::range_finder_poll()
-{
-	/* check if there is a range finder measurement */
-	bool range_finder_updated;
-	orb_check(_range_finder_sub, &range_finder_updated);
-
-	if (range_finder_updated) {
-		orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder);
-	}
-
-	return range_finder_updated;
-}
-
 void
 FixedwingPositionControl::vehicle_attitude_poll()
 {
@@ -846,21 +829,23 @@ void FixedwingPositionControl::navigation_capabilities_publish()
 	}
 }

-float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt)
+float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
 {
-	float rel_alt_estimated = current_alt - land_setpoint_alt;
-
-	/* only use range finder if:
-	 * parameter (range_finder_use_relative_alt) > 0
-	 * the measurement is valid
-	 * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
-	 */
-	if (range_finder_use_relative_alt < 0 || !range_finder.valid || range_finder.distance > range_finder_use_relative_alt ) {
-		return rel_alt_estimated;
+	if (!isfinite(global_pos.terrain_alt)) {
+		return land_setpoint_alt;
 	}

-	return range_finder.distance;
-
+	/* Decide if the terrain estimation can be used, once we switched to using the terrain we stick with it
+	 * for the whole landing */
+	if (_parameters.land_use_terrain_estimate && (global_pos.terrain_alt_valid || land_useterrain)) {
+		if(!land_useterrain) {
+			mavlink_log_info(_mavlink_fd, "#audio: Landing, using terrain estimate");
+			land_useterrain = true;
+		}
+		return global_pos.terrain_alt;
+	} else {
+		return land_setpoint_alt;
+	}
 }

 bool
@@ -965,10 +950,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_LAND) {

 			float bearing_lastwp_currwp = get_bearing_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
+			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));

 			/* Horizontal landing control */
 			/* switch to heading hold for the last meters, continue heading hold after */
 			float wp_distance = get_distance_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
+			/* calculate a waypoint distance value which is 0 when the aircraft is behind the waypoint */
+			float wp_distance_save = wp_distance;
+			if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) >= math::radians(90.0f)) {
+				wp_distance_save = 0.0f;
+			}
+
 			//warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO");
 			if (wp_distance < _parameters.land_heading_hold_horizontal_distance || land_noreturn_horizontal) {

@@ -1004,29 +996,30 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi

 			/* Vertical landing control */
 			//xxx: using the tecs altitude controller for slope control for now
-
-//				/* do not go down too early */
-//				if (wp_distance > 50.0f) {
-//					altitude_error = (_global_triplet.current.alt + 25.0f) - _global_pos.alt;
-//				}
 			/* 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

-			float flare_pitch_angle_rad = -math::radians(5.0f);//math::radians(pos_sp_triplet.current.param1)
 			float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f;
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;

-			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
-			float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
+			/* Get an estimate of the terrain altitude if available, otherwise terrain_alt will be
+			 * equal to _pos_sp_triplet.current.alt */
+			float terrain_alt = get_terrain_altitude_landing(_pos_sp_triplet.current.alt, _global_pos);
+
+			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
+			float L_altitude_rel = _pos_sp_triplet.previous.valid ?
+				_pos_sp_triplet.previous.alt - terrain_alt : 0.0f;

-			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
 			float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);

-			float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
-
-			if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
-
+			/* Check if we should start flaring with a vertical and a
+			 * horizontal limit (with some tolerance)
+			 * The horizontal limit is only applied when we are in front of the wp
+			 */
+			if (((_global_pos.alt < terrain_alt + landingslope.flare_relative_alt()) &&
+						(wp_distance_save < landingslope.flare_length() + 5.0f)) ||
+					land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
 				/* land with minimal speed */

 //					/* force TECS to only control speed with pitch, altitude is only implicitely controlled now */
@@ -1035,7 +1028,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* kill the throttle if param requests it */
 				throttle_max = _parameters.throttle_max;

-				 if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
+				 if (_global_pos.alt < terrain_alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 					if (!land_motor_lim) {
 						land_motor_lim  = true;
@@ -1053,12 +1046,13 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					land_stayonground = true;
 				}

-				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + flare_curve_alt_rel,
+				tecs_update_pitch_throttle(terrain_alt + flare_curve_alt_rel,
 						calculate_target_airspeed(airspeed_land), eas2tas,
-						flare_pitch_angle_rad, math::radians(15.0f),
+						 math::radians(_parameters.land_flare_pitch_min_deg),
+						 math::radians(_parameters.land_flare_pitch_max_deg),
 						0.0f, throttle_max, throttle_land,
-						false, flare_pitch_angle_rad,
-						_pos_sp_triplet.current.alt + relative_alt, ground_speed,
+						false,  land_motor_lim ? math::radians(_parameters.land_flare_pitch_min_deg) : math::radians(_parameters.pitch_limit_min),
+						_global_pos.alt, ground_speed,
 						land_motor_lim ? TECS_MODE_LAND_THROTTLELIM : TECS_MODE_LAND);

 				if (!land_noreturn_vertical) {
@@ -1079,8 +1073,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				  * if current position is below the slope continue at previous wp altitude
 				  * until the intersection with slope
 				  * */
-				float altitude_desired_rel = relative_alt;
-				if (relative_alt > landing_slope_alt_rel_desired || land_onslope) {
+				float altitude_desired_rel;
+				if (_global_pos.alt > terrain_alt + landing_slope_alt_rel_desired || land_onslope) {
 					/* stay on slope */
 					altitude_desired_rel = landing_slope_alt_rel_desired;
 					if (!land_onslope) {
@@ -1089,10 +1083,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					}
 				} else {
 					/* continue horizontally */
-					altitude_desired_rel =  _pos_sp_triplet.previous.valid ? L_altitude_rel : relative_alt;
+					altitude_desired_rel =  _pos_sp_triplet.previous.valid ? L_altitude_rel :
+						_global_pos.alt - terrain_alt;
 				}

-				tecs_update_pitch_throttle(_pos_sp_triplet.current.alt + altitude_desired_rel,
+				tecs_update_pitch_throttle(terrain_alt + altitude_desired_rel,
 						calculate_target_airspeed(airspeed_approach), eas2tas,
 						math::radians(_parameters.pitch_limit_min),
 						math::radians(_parameters.pitch_limit_max),
@@ -1101,7 +1096,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 						_parameters.throttle_cruise,
 						false,
 						math::radians(_parameters.pitch_limit_min),
-						_pos_sp_triplet.current.alt + relative_alt,
+						_global_pos.alt,
 						ground_speed);
 			}

@@ -1235,8 +1230,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		_att_sp.thrust = 0.0f;
 	} else if (pos_sp_triplet.current.type == SETPOINT_TYPE_TAKEOFF &&
 			launch_detection_state != LAUNCHDETECTION_RES_DETECTED_ENABLEMOTORS) {
-		/* Copy thrust and pitch values from tecs
-		 * making sure again that the correct thrust is used,
+		 /* making sure again that the correct thrust is used,
 		 * without depending on library calls for safety reasons */
 		_att_sp.thrust = launchDetector.getThrottlePreTakeoff();
 	} else {
@@ -1278,7 +1272,6 @@ FixedwingPositionControl::task_main()
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
-	_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));

 	/* rate limit control mode updates to 5Hz */
 	orb_set_interval(_control_mode_sub, 200);
@@ -1357,7 +1350,6 @@ FixedwingPositionControl::task_main()
 			vehicle_setpoint_poll();
 			vehicle_sensor_combined_poll();
 			vehicle_airspeed_poll();
-			range_finder_poll();
 			// vehicle_baro_poll();

 			math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e,  _global_pos.vel_d);
@@ -1421,6 +1413,7 @@ void FixedwingPositionControl::reset_landing_state()
 	land_stayonground = false;
 	land_motor_lim = false;
 	land_onslope = false;
+	land_useterrain = false;
 }

 void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_sp, float eas2tas,
@@ -412,12 +412,28 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, -1.0f);
 PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);

 /**
- * Relative altitude threshold for range finder measurements for use during landing
+ * Enable or disable usage of terrain estimate during landing
 *
- * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT
- * set to < 0 to disable
- * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location
+ * 0: disabled, 1: enabled
 *
 * @group L1 Control
 */
-PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
+PARAM_DEFINE_INT32(FW_LND_USETER, 0);
+
+/**
+ * Flare, minimum pitch
+ *
+ * Minimum pitch during flare, a positive sign means nose up
+ * Applied once FW_LND_TLALT is reached
+ *
+ */
+PARAM_DEFINE_FLOAT(FW_FLARE_PMIN, 2.5f);
+
+/**
+ * Flare, maximum pitch
+ *
+ * Maximum pitch during flare, a positive sign means nose up
+ * Applied once FW_LND_TLALT is reached
+ *
+ */
+PARAM_DEFINE_FLOAT(FW_FLARE_PMAX, 15.0f);
@@ -68,6 +68,13 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
 * @group MAVLink
 */
 PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
+/**
+ * Forward external setpoint messages
+ * If set to 1 incomming external setpoint messages will be directly forwarded to the controllers if in offboard
+ * control mode
+ * @group MAVLink
+ */
+PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);

 mavlink_system_t mavlink_system = {
 	100,
@@ -34,6 +34,7 @@
 /// @file mavlink_ftp.cpp
 ///	@author px4dev, Don Gagne <don@thegagnes.com>

+#include <crc32.h>
 #include <unistd.h>
 #include <stdio.h>
 #include <fcntl.h>
@@ -180,12 +181,16 @@ MavlinkFTP::_process_request(Request *req)
 		errorCode = _workList(payload);
 		break;

-	case kCmdOpenFile:
-		errorCode = _workOpen(payload, false);
+	case kCmdOpenFileRO:
+		errorCode = _workOpen(payload, O_RDONLY);
 		break;

 	case kCmdCreateFile:
-		errorCode = _workOpen(payload, true);
+		errorCode = _workOpen(payload, O_CREAT | O_EXCL | O_WRONLY);
+		break;
+
+	case kCmdOpenFileWO:
+		errorCode = _workOpen(payload, O_CREAT | O_WRONLY);
 		break;

 	case kCmdReadFile:
@@ -200,6 +205,14 @@ MavlinkFTP::_process_request(Request *req)
 		errorCode = _workRemoveFile(payload);
 		break;

+	case kCmdRename:
+		errorCode = _workRename(payload);
+		break;
+
+	case kCmdTruncateFile:
+		errorCode = _workTruncateFile(payload);
+		break;
+
 	case kCmdCreateDirectory:
 		errorCode = _workCreateDirectory(payload);
 		break;
@@ -208,6 +221,11 @@ MavlinkFTP::_process_request(Request *req)
 		errorCode = _workRemoveDirectory(payload);
 		break;
 			
+
+	case kCmdCalcFileCRC32:
+		errorCode = _workCalcFileCRC32(payload);
+		break;
+
 	default:
 		errorCode = kErrUnknownCommand;
 		break;	
@@ -222,6 +240,7 @@ out:
 		warnx("FTP: ack\n");
 #endif
 	} else {
+		int r_errno = errno;
 		warnx("FTP: nak %u", errorCode);
 		payload->req_opcode = payload->opcode;
 		payload->opcode = kRspNak;
@@ -229,7 +248,7 @@ out:
 		payload->data[0] = errorCode;
 		if (errorCode == kErrFailErrno) {
 			payload->size = 2;
-			payload->data[1] = errno;
+			payload->data[1] = r_errno;
 		}
 	}

@@ -396,27 +415,27 @@ MavlinkFTP::_workList(PayloadHeader* payload)

 /// @brief Responds to an Open command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
+MavlinkFTP::_workOpen(PayloadHeader* payload, int oflag)
 {
 	int session_index = _find_unused_session();
 	if (session_index < 0) {
 		warnx("FTP: Open failed - out of sessions\n");
 		return kErrNoSessionsAvailable;
 	}
-	
-	char *filename = _data_as_cstring(payload);
-	
-	uint32_t fileSize = 0;
-	if (!create) {
-		struct stat st;
-		if (stat(filename, &st) != 0) {
-			return kErrFailErrno;
-		}
-		fileSize = st.st_size;
-	}

-	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
-    
+	char *filename = _data_as_cstring(payload);
+
+	uint32_t fileSize = 0;
+	struct stat st;
+	if (stat(filename, &st) != 0) {
+		// fail only if requested open for read
+		if (oflag & O_RDONLY)
+			return kErrFailErrno;
+		else
+			st.st_size = 0;
+	}
+	fileSize = st.st_size;
+
 	int fd = ::open(filename, oflag);
 	if (fd < 0) {
 		return kErrFailErrno;
@@ -424,12 +443,8 @@ MavlinkFTP::_workOpen(PayloadHeader* payload, bool create)
 	_session_fds[session_index] = fd;

 	payload->session = session_index;
-	if (create) {
-		payload->size = 0;
-	} else {
-		payload->size = sizeof(uint32_t);
-		*((uint32_t*)payload->data) = fileSize;
-	}
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = fileSize;

 	return kErrNone;
 }
@@ -470,29 +485,33 @@ MavlinkFTP::_workRead(PayloadHeader* payload)
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workWrite(PayloadHeader* payload)
 {
-#if 0
-    // NYI: Coming soon
-	auto hdr = req->header();
+	int session_index = payload->session;

-	// look up session
-	auto session = getSession(hdr->session);
-	if (session == nullptr) {
-		return kErrNoSession;
+	if (!_valid_session(session_index)) {
+		return kErrInvalidSession;
 	}

-	// append to file
-	int result = session->append(hdr->offset, &hdr->data[0], hdr->size);
-
-	if (result < 0) {
-		// XXX might also be no space, I/O, etc.
-		return kErrNotAppend;
-	}
-
-	hdr->size = result;
-	return kErrNone;
-#else
-	return kErrUnknownCommand;
+	// Seek to the specified position
+#ifdef MAVLINK_FTP_DEBUG
+	warnx("seek %d", payload->offset);
 #endif
+	if (lseek(_session_fds[session_index], payload->offset, SEEK_SET) < 0) {
+		// Unable to see to the specified location
+		warnx("seek fail");
+		return kErrFailErrno;
+	}
+
+	int bytes_written = ::write(_session_fds[session_index], &payload->data[0], payload->size);
+	if (bytes_written < 0) {
+		// Negative return indicates error other than eof
+		warnx("write fail %d", bytes_written);
+		return kErrFailErrno;
+	}
+
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = bytes_written;
+
+	return kErrNone;
 }

 /// @brief Responds to a RemoveFile command
@@ -510,6 +529,81 @@ MavlinkFTP::_workRemoveFile(PayloadHeader* payload)
 	}
 }

+/// @brief Responds to a TruncateFile command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workTruncateFile(PayloadHeader* payload)
+{
+	char file[kMaxDataLength];
+	const char temp_file[] = "/fs/microsd/.trunc.tmp";
+	strncpy(file, _data_as_cstring(payload), kMaxDataLength);
+	payload->size = 0;
+
+	// emulate truncate(file, payload->offset) by
+	// copying to temp and overwrite with O_TRUNC flag.
+
+	struct stat st;
+	if (stat(file, &st) != 0) {
+		return kErrFailErrno;
+	}
+
+	if (!S_ISREG(st.st_mode)) {
+		errno = EISDIR;
+		return kErrFailErrno;
+	}
+
+	// check perms allow us to write (not romfs)
+	if (!(st.st_mode & (S_IWUSR | S_IWGRP | S_IWOTH))) {
+		errno = EROFS;
+		return kErrFailErrno;
+	}
+
+	if (payload->offset == (unsigned)st.st_size) {
+		// nothing to do
+		return kErrNone;
+	}
+	else if (payload->offset == 0) {
+		// 1: truncate all data
+		int fd = ::open(file, O_TRUNC | O_WRONLY);
+		if (fd < 0) {
+			return kErrFailErrno;
+		}
+
+		::close(fd);
+		return kErrNone;
+	}
+	else if (payload->offset > (unsigned)st.st_size) {
+		// 2: extend file
+		int fd = ::open(file, O_WRONLY);
+		if (fd < 0) {
+			return kErrFailErrno;
+		}
+
+		if (lseek(fd, payload->offset - 1, SEEK_SET) < 0) {
+			::close(fd);
+			return kErrFailErrno;
+		}
+
+		bool ok = 1 == ::write(fd, "", 1);
+		::close(fd);
+
+		return (ok)? kErrNone : kErrFailErrno;
+	}
+	else {
+		// 3: truncate
+		if (_copy_file(file, temp_file, payload->offset) != 0) {
+			return kErrFailErrno;
+		}
+		if (_copy_file(temp_file, file, payload->offset) != 0) {
+			return kErrFailErrno;
+		}
+		if (::unlink(temp_file) != 0) {
+			return kErrFailErrno;
+		}
+
+		return kErrNone;
+	}
+}
+
 /// @brief Responds to a Terminate command
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workTerminate(PayloadHeader* payload)
@@ -542,6 +636,33 @@ MavlinkFTP::_workReset(PayloadHeader* payload)
 	return kErrNone;
 }

+/// @brief Responds to a Rename command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workRename(PayloadHeader* payload)
+{
+	char oldpath[kMaxDataLength];
+	char newpath[kMaxDataLength];
+
+	char *ptr = _data_as_cstring(payload);
+	size_t oldpath_sz = strlen(ptr);
+
+	if (oldpath_sz == payload->size) {
+		// no newpath
+		errno = EINVAL;
+		return kErrFailErrno;
+	}
+
+	strncpy(oldpath, ptr, kMaxDataLength);
+	strncpy(newpath, ptr + oldpath_sz + 1, kMaxDataLength);
+
+	if (rename(oldpath, newpath) == 0) {
+		payload->size = 0;
+		return kErrNone;
+	} else {
+		return kErrFailErrno;
+	}
+}
+
 /// @brief Responds to a RemoveDirectory command
 MavlinkFTP::ErrorCode
 MavlinkFTP::_workRemoveDirectory(PayloadHeader* payload)
@@ -572,6 +693,39 @@ MavlinkFTP::_workCreateDirectory(PayloadHeader* payload)
 	}
 }

+/// @brief Responds to a CalcFileCRC32 command
+MavlinkFTP::ErrorCode
+MavlinkFTP::_workCalcFileCRC32(PayloadHeader* payload)
+{
+	char file_buf[256];
+	uint32_t checksum = 0;
+	ssize_t bytes_read;
+	strncpy(file_buf, _data_as_cstring(payload), kMaxDataLength);
+
+	int fd = ::open(file_buf, O_RDONLY);
+	if (fd < 0) {
+		return kErrFailErrno;
+	}
+
+	do {
+		bytes_read = ::read(fd, file_buf, sizeof(file_buf));
+		if (bytes_read < 0) {
+			int r_errno = errno;
+			::close(fd);
+			errno = r_errno;
+			return kErrFailErrno;
+		}
+
+		checksum = crc32part((uint8_t*)file_buf, bytes_read, checksum);
+	} while (bytes_read == sizeof(file_buf));
+
+	::close(fd);
+
+	payload->size = sizeof(uint32_t);
+	*((uint32_t*)payload->data) = checksum;
+	return kErrNone;
+}
+
 /// @brief Returns true if the specified session is a valid open session
 bool
 MavlinkFTP::_valid_session(unsigned index)
@@ -645,3 +799,55 @@ MavlinkFTP::_return_request(Request *req)
 	_unlock_request_queue();
 }

+/// @brief Copy file (with limited space)
+int
+MavlinkFTP::_copy_file(const char *src_path, const char *dst_path, ssize_t length)
+{
+	char buff[512];
+	int src_fd = -1, dst_fd = -1;
+	int op_errno = 0;
+
+	src_fd = ::open(src_path, O_RDONLY);
+	if (src_fd < 0) {
+		return -1;
+	}
+
+	dst_fd = ::open(dst_path, O_CREAT | O_TRUNC | O_WRONLY);
+	if (dst_fd < 0) {
+		op_errno = errno;
+		::close(src_fd);
+		errno = op_errno;
+		return -1;
+	}
+
+	while (length > 0) {
+		ssize_t bytes_read, bytes_written;
+		size_t blen = (length > sizeof(buff))? sizeof(buff) : length;
+
+		bytes_read = ::read(src_fd, buff, blen);
+		if (bytes_read == 0) {
+			// EOF
+			break;
+		}
+		else if (bytes_read < 0) {
+			warnx("cp: read");
+			op_errno = errno;
+			break;
+		}
+
+		bytes_written = ::write(dst_fd, buff, bytes_read);
+		if (bytes_written != bytes_read) {
+			warnx("cp: short write");
+			op_errno = errno;
+			break;
+		}
+
+		length -= bytes_written;
+	}
+
+	::close(src_fd);
+	::close(dst_fd);
+
+	errno = op_errno;
+	return (length > 0)? -1 : 0;
+}
@@ -89,13 +89,17 @@ public:
 		kCmdTerminateSession,	///< Terminates open Read session
 		kCmdResetSessions,	///< Terminates all open Read sessions
 		kCmdListDirectory,	///< List files in <path> from <offset>
-		kCmdOpenFile,		///< Opens file at <path> for reading, returns <session>
+		kCmdOpenFileRO,		///< Opens file at <path> for reading, returns <session>
 		kCmdReadFile,		///< Reads <size> bytes from <offset> in <session>
 		kCmdCreateFile,		///< Creates file at <path> for writing, returns <session>
-		kCmdWriteFile,		///< Appends <size> bytes to file in <session>
+		kCmdWriteFile,		///< Writes <size> bytes to <offset> in <session>
 		kCmdRemoveFile,		///< Remove file at <path>
 		kCmdCreateDirectory,	///< Creates directory at <path>
 		kCmdRemoveDirectory,	///< Removes Directory at <path>, must be empty
+		kCmdOpenFileWO,		///< Opens file at <path> for writing, returns <session>
+		kCmdTruncateFile,	///< Truncate file at <path> to <offset> length
+		kCmdRename,		///< Rename <path1> to <path2>
+		kCmdCalcFileCRC32,	///< Calculate CRC32 for file at <path>
 		
 		kRspAck = 128,		///< Ack response
 		kRspNak			///< Nak response
@@ -138,9 +142,10 @@ private:
 	static void	_worker_trampoline(void *arg);
 	void		_process_request(Request *req);
 	void		_reply(Request *req);
+	int		_copy_file(const char *src_path, const char *dst_path, ssize_t length);

 	ErrorCode	_workList(PayloadHeader *payload);
-	ErrorCode	_workOpen(PayloadHeader *payload, bool create);
+	ErrorCode	_workOpen(PayloadHeader *payload, int oflag);
 	ErrorCode	_workRead(PayloadHeader *payload);
 	ErrorCode	_workWrite(PayloadHeader *payload);
 	ErrorCode	_workTerminate(PayloadHeader *payload);
@@ -148,6 +153,9 @@ private:
 	ErrorCode	_workRemoveDirectory(PayloadHeader *payload);
 	ErrorCode	_workCreateDirectory(PayloadHeader *payload);
 	ErrorCode	_workRemoveFile(PayloadHeader *payload);
+	ErrorCode	_workTruncateFile(PayloadHeader *payload);
+	ErrorCode	_workRename(PayloadHeader *payload);
+	ErrorCode	_workCalcFileCRC32(PayloadHeader *payload);

 	static const unsigned	kRequestQueueSize = 2;			///< Max number of queued requests
 	Request			_request_bufs[kRequestQueueSize];	///< Request buffers which hold work
@@ -123,6 +123,7 @@ Mavlink::Mavlink() :
 	_task_running(false),
 	_hil_enabled(false),
 	_use_hil_gps(false),
+	_forward_externalsp(false),
 	_is_usb_uart(false),
 	_wait_to_transmit(false),
 	_received_messages(false),
@@ -483,6 +484,7 @@ void Mavlink::mavlink_update_system(void)
 		_param_component_id = param_find("MAV_COMP_ID");
 		_param_system_type = param_find("MAV_TYPE");
 		_param_use_hil_gps = param_find("MAV_USEHILGPS");
+		_param_forward_externalsp = param_find("MAV_FWDEXTSP");
 	}

 	/* update system and component id */
@@ -529,6 +531,11 @@ void Mavlink::mavlink_update_system(void)
 	param_get(_param_use_hil_gps, &use_hil_gps);

 	_use_hil_gps = (bool)use_hil_gps;
+
+	int32_t forward_externalsp;
+	param_get(_param_forward_externalsp, &forward_externalsp);
+
+	_forward_externalsp = (bool)forward_externalsp;
 }

 int Mavlink::get_system_id()
@@ -1396,7 +1403,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("POSITION_TARGET_GLOBAL_INT", 3.0f);
 		configure_stream("ATTITUDE_TARGET", 3.0f);
 		configure_stream("DISTANCE_SENSOR", 0.5f);
-		configure_stream("OPTICAL_FLOW", 20.0f);
+		configure_stream("OPTICAL_FLOW", 5.0f);
 		break;

 	case MAVLINK_MODE_ONBOARD:
@@ -1404,6 +1411,9 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("ATTITUDE", 50.0f);
 		configure_stream("GLOBAL_POSITION_INT", 50.0f);
 		configure_stream("CAMERA_CAPTURE", 2.0f);
+		configure_stream("ATTITUDE_TARGET", 10.0f);
+		configure_stream("POSITION_TARGET_GLOBAL_INT", 10.0f);
+		configure_stream("VFR_HUD", 10.0f);
 		break;

 	default:
@@ -137,6 +137,8 @@ public:

 	bool			get_use_hil_gps() { return _use_hil_gps; }

+	bool			get_forward_externalsp() { return _forward_externalsp; }
+
 	bool			get_flow_control_enabled() { return _flow_control_enabled; }

 	bool			get_forwarding_on() { return _forwarding_on; }
@@ -232,7 +234,7 @@ public:
 	bool			should_transmit() { return (!_wait_to_transmit || (_wait_to_transmit && _received_messages)); }

 	bool			message_buffer_write(const void *ptr, int size);
-    
+
 	void			lockMessageBufferMutex(void) { pthread_mutex_lock(&_message_buffer_mutex); }
 	void			unlockMessageBufferMutex(void) { pthread_mutex_unlock(&_message_buffer_mutex); }

@@ -275,6 +277,7 @@ private:
 	/* states */
 	bool			_hil_enabled;		/**< Hardware In the Loop mode */
 	bool			_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
+	bool			_forward_externalsp;	/**< Forward external setpoint messages to controllers directly if in offboard mode */
 	bool			_is_usb_uart;		/**< Port is USB */
 	bool        		_wait_to_transmit;  	/**< Wait to transmit until received messages. */
 	bool        		_received_messages;	/**< Whether we've received valid mavlink messages. */
@@ -349,6 +352,7 @@ private:
 	param_t			_param_component_id;
 	param_t			_param_system_type;
 	param_t			_param_use_hil_gps;
+	param_t			_param_forward_externalsp;

 	perf_counter_t		_loop_perf;			/**< loop performance counter */
 	perf_counter_t		_txerr_perf;			/**< TX error counter */
@@ -37,6 +37,7 @@
 *
 * @author Lorenz Meier <lm@inf.ethz.ch>
 * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
 */

 /* XXX trim includes */
@@ -105,10 +106,11 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_flow_pub(-1),
 	_range_pub(-1),
 	_offboard_control_sp_pub(-1),
-	_local_pos_sp_pub(-1),
 	_global_vel_sp_pub(-1),
 	_att_sp_pub(-1),
 	_rates_sp_pub(-1),
+	_force_sp_pub(-1),
+	_pos_sp_triplet_pub(-1),
 	_vicon_position_pub(-1),
 	_vision_position_pub(-1),
 	_telemetry_status_pub(-1),
@@ -154,6 +156,14 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_vicon_position_estimate(msg);
 		break;

+	case MAVLINK_MSG_ID_SET_POSITION_TARGET_LOCAL_NED:
+		handle_message_set_position_target_local_ned(msg);
+		break;
+
+	case MAVLINK_MSG_ID_SET_ATTITUDE_TARGET:
+		handle_message_set_attitude_target(msg);
+		break;
+
 	case MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE:
 		handle_message_vision_position_estimate(msg);
 		break;
@@ -474,6 +484,189 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 	}
 }

+void
+MavlinkReceiver::handle_message_set_position_target_local_ned(mavlink_message_t *msg)
+{
+	mavlink_set_position_target_local_ned_t set_position_target_local_ned;
+	mavlink_msg_set_position_target_local_ned_decode(msg, &set_position_target_local_ned);
+
+	struct offboard_control_setpoint_s offboard_control_sp;
+	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));//XXX breaks compatibility with multiple setpoints
+
+	/* Only accept messages which are intended for this system */
+	if ((mavlink_system.sysid == set_position_target_local_ned.target_system ||
+				set_position_target_local_ned.target_system == 0) &&
+			(mavlink_system.compid == set_position_target_local_ned.target_component ||
+			 set_position_target_local_ned.target_component == 0)) {
+
+		/* convert mavlink type (local, NED) to uORB offboard control struct */
+		switch (set_position_target_local_ned.coordinate_frame) {
+			case MAV_FRAME_LOCAL_NED:
+				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED;
+				break;
+			case MAV_FRAME_LOCAL_OFFSET_NED:
+				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED;
+				break;
+			case MAV_FRAME_BODY_NED:
+				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED;
+				break;
+			case MAV_FRAME_BODY_OFFSET_NED:
+				offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED;
+				break;
+			default:
+				/* invalid setpoint, avoid publishing */
+				return;
+		}
+		offboard_control_sp.position[0] = set_position_target_local_ned.x;
+		offboard_control_sp.position[1] = set_position_target_local_ned.y;
+		offboard_control_sp.position[2] = set_position_target_local_ned.z;
+		offboard_control_sp.velocity[0] = set_position_target_local_ned.vx;
+		offboard_control_sp.velocity[1] = set_position_target_local_ned.vy;
+		offboard_control_sp.velocity[2] = set_position_target_local_ned.vz;
+		offboard_control_sp.acceleration[0] = set_position_target_local_ned.afx;
+		offboard_control_sp.acceleration[1] = set_position_target_local_ned.afy;
+		offboard_control_sp.acceleration[2] = set_position_target_local_ned.afz;
+		offboard_control_sp.yaw = set_position_target_local_ned.yaw;
+		offboard_control_sp.yaw_rate = set_position_target_local_ned.yaw_rate;
+		offboard_control_sp.isForceSetpoint = (bool)(set_position_target_local_ned.type_mask & (1 << 9));
+
+		/* If we are in force control mode, for now set offboard mode to force control */
+		if (offboard_control_sp.isForceSetpoint) {
+			offboard_control_sp.mode = OFFBOARD_CONTROL_MODE_DIRECT_FORCE;
+		}
+
+		/* set ignore flags */
+		for (int i = 0; i < 9; i++) {
+			offboard_control_sp.ignore &=  ~(1 << i);
+			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << i));
+		}
+		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAW);
+			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 10)) <<
+					OFB_IGN_BIT_YAW;
+		offboard_control_sp.ignore &=  ~(1 << OFB_IGN_BIT_YAWRATE);
+			offboard_control_sp.ignore |=  (set_position_target_local_ned.type_mask & (1 << 11)) <<
+					OFB_IGN_BIT_YAWRATE;
+
+		offboard_control_sp.timestamp = hrt_absolute_time();
+
+		if (_offboard_control_sp_pub < 0) {
+			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+
+		} else {
+			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+		}
+
+		/* If we are in offboard control mode and offboard control loop through is enabled
+		 * also publish the setpoint topic which is read by the controller */
+		if (_mavlink->get_forward_externalsp()) {
+			bool updated;
+			orb_check(_control_mode_sub, &updated);
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+			}
+			if (_control_mode.flag_control_offboard_enabled) {
+				if (offboard_control_sp.isForceSetpoint &&
+						offboard_control_sp_ignore_position_all(offboard_control_sp) &&
+						offboard_control_sp_ignore_velocity_all(offboard_control_sp)) {
+					/* The offboard setpoint is a force setpoint only, directly writing to the force
+					 * setpoint topic and not publishing the setpoint triplet topic */
+					struct vehicle_force_setpoint_s	force_sp;
+					force_sp.x = offboard_control_sp.acceleration[0];
+					force_sp.y = offboard_control_sp.acceleration[1];
+					force_sp.z = offboard_control_sp.acceleration[2];
+					//XXX: yaw
+					if (_force_sp_pub < 0) {
+						_force_sp_pub = orb_advertise(ORB_ID(vehicle_force_setpoint), &force_sp);
+					} else {
+						orb_publish(ORB_ID(vehicle_force_setpoint), _force_sp_pub, &force_sp);
+					}
+				} else {
+					/* It's not a pure force setpoint: publish to setpoint triplet  topic */
+					struct position_setpoint_triplet_s pos_sp_triplet;
+					pos_sp_triplet.previous.valid = false;
+					pos_sp_triplet.next.valid = false;
+					pos_sp_triplet.current.valid = true;
+					pos_sp_triplet.current.type = SETPOINT_TYPE_POSITION; //XXX support others
+
+					/* set the local pos values if the setpoint type is 'local pos' and none
+					 * of the local pos fields is set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_position_some(offboard_control_sp)) {
+					pos_sp_triplet.current.position_valid = true;
+					pos_sp_triplet.current.x = offboard_control_sp.position[0];
+					pos_sp_triplet.current.y = offboard_control_sp.position[1];
+					pos_sp_triplet.current.z = offboard_control_sp.position[2];
+					} else {
+						pos_sp_triplet.current.position_valid = false;
+					}
+
+					/* set the local vel values if the setpoint type is 'local pos' and none
+					 * of the local vel fields is set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_velocity_some(offboard_control_sp)) {
+						pos_sp_triplet.current.velocity_valid = true;
+						pos_sp_triplet.current.vx = offboard_control_sp.velocity[0];
+						pos_sp_triplet.current.vy = offboard_control_sp.velocity[1];
+						pos_sp_triplet.current.vz = offboard_control_sp.velocity[2];
+					} else {
+						pos_sp_triplet.current.velocity_valid = false;
+					}
+
+					/* set the local acceleration values if the setpoint type is 'local pos' and none
+					 * of the accelerations fields is set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_acceleration_some(offboard_control_sp)) {
+						pos_sp_triplet.current.acceleration_valid = true;
+						pos_sp_triplet.current.a_x = offboard_control_sp.acceleration[0];
+						pos_sp_triplet.current.a_y = offboard_control_sp.acceleration[1];
+						pos_sp_triplet.current.a_z = offboard_control_sp.acceleration[2];
+						pos_sp_triplet.current.acceleration_is_force =
+						offboard_control_sp.isForceSetpoint;
+
+					} else {
+						pos_sp_triplet.current.acceleration_valid = false;
+					}
+
+					/* set the yaw sp value if the setpoint type is 'local pos' and the yaw
+					 * field is not set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_yaw(offboard_control_sp)) {
+						pos_sp_triplet.current.yaw_valid = true;
+						pos_sp_triplet.current.yaw = offboard_control_sp.yaw;
+
+					} else {
+						pos_sp_triplet.current.yaw_valid = false;
+					}
+
+					/* set the yawrate sp value if the setpoint type is 'local pos' and the yawrate
+					 * field is not set to 'ignore' */
+					if (offboard_control_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_NED &&
+							!offboard_control_sp_ignore_yawrate(offboard_control_sp)) {
+						pos_sp_triplet.current.yawspeed_valid = true;
+						pos_sp_triplet.current.yawspeed = offboard_control_sp.yaw_rate;
+
+					} else {
+						pos_sp_triplet.current.yawspeed_valid = false;
+					}
+					//XXX handle global pos setpoints (different MAV frames)
+
+
+					if (_pos_sp_triplet_pub < 0) {
+						_pos_sp_triplet_pub = orb_advertise(ORB_ID(position_setpoint_triplet),
+								&pos_sp_triplet);
+					} else {
+						orb_publish(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_pub,
+								&pos_sp_triplet);
+					}
+
+				}
+
+			}
+
+		}
+	}
+}
+
 void
 MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 {
@@ -513,6 +706,103 @@ MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 	}
 }

+void
+MavlinkReceiver::handle_message_set_attitude_target(mavlink_message_t *msg)
+{
+	mavlink_set_attitude_target_t set_attitude_target;
+	mavlink_msg_set_attitude_target_decode(msg, &set_attitude_target);
+
+	struct offboard_control_setpoint_s offboard_control_sp;
+	memset(&offboard_control_sp, 0, sizeof(offboard_control_sp)); //XXX breaks compatibility with multiple setpoints
+
+	/* Only accept messages which are intended for this system */
+	if ((mavlink_system.sysid == set_attitude_target.target_system ||
+				set_attitude_target.target_system == 0) &&
+			(mavlink_system.compid == set_attitude_target.target_component ||
+			 set_attitude_target.target_component == 0)) {
+		for (int i = 0; i < 4; i++) {
+			offboard_control_sp.attitude[i] = set_attitude_target.q[i];
+		}
+		offboard_control_sp.attitude_rate[0] = set_attitude_target.body_roll_rate;
+		offboard_control_sp.attitude_rate[1] = set_attitude_target.body_pitch_rate;
+		offboard_control_sp.attitude_rate[2] = set_attitude_target.body_yaw_rate;
+
+		/* set correct ignore flags for body rate fields: copy from mavlink message */
+		for (int i = 0; i < 3; i++) {
+			offboard_control_sp.ignore &=  ~(1 << (i + OFB_IGN_BIT_BODYRATE_X));
+			offboard_control_sp.ignore |=  (set_attitude_target.type_mask & (1 << i)) << OFB_IGN_BIT_BODYRATE_X;
+		}
+		/* set correct ignore flags for thrust field: copy from mavlink message */
+		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_THRUST);
+		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 6)) << OFB_IGN_BIT_THRUST);
+		/* set correct ignore flags for attitude field: copy from mavlink message */
+		offboard_control_sp.ignore &= ~(1 << OFB_IGN_BIT_ATT);
+		offboard_control_sp.ignore |= ((set_attitude_target.type_mask & (1 << 7)) << OFB_IGN_BIT_ATT);
+
+
+		offboard_control_sp.timestamp = hrt_absolute_time();
+		offboard_control_sp.mode =OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE; //XXX handle rate control mode
+
+		if (_offboard_control_sp_pub < 0) {
+			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
+
+		} else {
+			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
+		}
+
+		/* If we are in offboard control mode and offboard control loop through is enabled
+		 * also publish the setpoint topic which is read by the controller */
+		if (_mavlink->get_forward_externalsp()) {
+			bool updated;
+			orb_check(_control_mode_sub, &updated);
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
+			}
+
+			if (_control_mode.flag_control_offboard_enabled) {
+
+				/* Publish attitude setpoint if attitude and thrust ignore bits are not set */
+				if (!(offboard_control_sp_ignore_attitude(offboard_control_sp) ||
+							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
+					struct vehicle_attitude_setpoint_s att_sp;
+					att_sp.timestamp = hrt_absolute_time();
+					mavlink_quaternion_to_euler(set_attitude_target.q,
+							&att_sp.roll_body, &att_sp.pitch_body, &att_sp.yaw_body);
+					mavlink_quaternion_to_dcm(set_attitude_target.q, att_sp.R_body);
+					att_sp.R_valid = true;
+					att_sp.thrust = set_attitude_target.thrust;
+					memcpy(att_sp.q_d, set_attitude_target.q, sizeof(att_sp.q_d));
+					att_sp.q_d_valid = true;
+					if (_att_sp_pub < 0) {
+						_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp);
+					} else {
+						orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub, &att_sp);
+					}
+				}
+
+				/* Publish attitude rate setpoint if bodyrate and thrust ignore bits are not set */
+				///XXX add support for ignoring individual axes
+				if (!(offboard_control_sp_ignore_bodyrates_some(offboard_control_sp) ||
+							offboard_control_sp_ignore_thrust(offboard_control_sp))) {
+					struct vehicle_rates_setpoint_s rates_sp;
+					rates_sp.timestamp = hrt_absolute_time();
+					rates_sp.roll = set_attitude_target.body_roll_rate;
+					rates_sp.pitch = set_attitude_target.body_pitch_rate;
+					rates_sp.yaw = set_attitude_target.body_yaw_rate;
+					rates_sp.thrust = set_attitude_target.thrust;
+
+					if (_att_sp_pub < 0) {
+						_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+					} else {
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _rates_sp_pub, &rates_sp);
+					}
+				}
+			}
+
+		}
+	}
+}
+
 void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {
@@ -71,6 +71,7 @@
 #include <uORB/topics/debug_key_value.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/vehicle_force_setpoint.h>

 #include "mavlink_ftp.h"

@@ -117,6 +118,8 @@ private:
 	void handle_message_vicon_position_estimate(mavlink_message_t *msg);
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
+	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
+	void handle_message_set_attitude_target(mavlink_message_t *msg);
 	void handle_message_radio_status(mavlink_message_t *msg);
 	void handle_message_manual_control(mavlink_message_t *msg);
 	void handle_message_heartbeat(mavlink_message_t *msg);
@@ -145,10 +148,11 @@ private:
 	orb_advert_t _flow_pub;
 	orb_advert_t _range_pub;
 	orb_advert_t _offboard_control_sp_pub;
-	orb_advert_t _local_pos_sp_pub;
 	orb_advert_t _global_vel_sp_pub;
 	orb_advert_t _att_sp_pub;
 	orb_advert_t _rates_sp_pub;
+	orb_advert_t _force_sp_pub;
+	orb_advert_t _pos_sp_triplet_pub;
 	orb_advert_t _vicon_position_pub;
 	orb_advert_t _vision_position_pub;
 	orb_advert_t _telemetry_status_pub;
@@ -65,7 +65,7 @@ MavlinkFtpTest::~MavlinkFtpTest()
 }

 /// @brief Called before every test to initialize the FTP Server.
-void MavlinkFtpTest::init(void)
+void MavlinkFtpTest::_init(void)
 {
 	_ftp_server = new MavlinkFTP;;
 	_ftp_server->set_unittest_worker(MavlinkFtpTest::receive_message, this);
@@ -74,7 +74,7 @@ void MavlinkFtpTest::init(void)
 }

 /// @brief Called after every test to take down the FTP Server.
-void MavlinkFtpTest::cleanup(void)
+void MavlinkFtpTest::_cleanup(void)
 {
 	delete _ftp_server;
 	
@@ -265,7 +265,7 @@ bool MavlinkFtpTest::_open_badfile_test(void)
 	MavlinkFTP::PayloadHeader		*reply;
 	const char				*dir = "/foo";	// non-existent file
 	
-	payload.opcode = MavlinkFTP::kCmdOpenFile;
+	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
 	
 	bool success = _send_receive_msg(&payload,	// FTP payload header
@@ -295,7 +295,7 @@ bool MavlinkFtpTest::_open_terminate_test(void)
 		struct stat st;
 		const ReadTestCase *test = &_rgReadTestCases[i];
 		
-		payload.opcode = MavlinkFTP::kCmdOpenFile;
+		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 		payload.offset = 0;
 		
 		bool success = _send_receive_msg(&payload,		// FTP payload header
@@ -342,7 +342,7 @@ bool MavlinkFtpTest::_terminate_badsession_test(void)
 	MavlinkFTP::PayloadHeader		*reply;
 	const char				*file = _rgReadTestCases[0].file;
 	
-	payload.opcode = MavlinkFTP::kCmdOpenFile;
+	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
 	
 	bool success = _send_receive_msg(&payload,	// FTP payload header
@@ -400,7 +400,7 @@ bool MavlinkFtpTest::_read_test(void)
 		// Test case data files are created for specific boundary conditions
 		ut_compare("Test case data files are out of date", test->length, st.st_size);
 		
-		payload.opcode = MavlinkFTP::kCmdOpenFile;
+		payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 		payload.offset = 0;
 		
 		bool success = _send_receive_msg(&payload,		// FTP payload header
@@ -463,7 +463,7 @@ bool MavlinkFtpTest::_read_badsession_test(void)
 	MavlinkFTP::PayloadHeader		*reply;
 	const char				*file = _rgReadTestCases[0].file;
 	
-	payload.opcode = MavlinkFTP::kCmdOpenFile;
+	payload.opcode = MavlinkFTP::kCmdOpenFileRO;
 	payload.offset = 0;
 	
 	bool success = _send_receive_msg(&payload,	// FTP payload header
@@ -738,7 +738,7 @@ void MavlinkFtpTest::_cleanup_microsd(void)
 }

 /// @brief Runs all the unit tests
-void MavlinkFtpTest::runTests(void)
+bool MavlinkFtpTest::run_tests(void)
 {
 	ut_run_test(_ack_test);
 	ut_run_test(_bad_opcode_test);
@@ -753,5 +753,9 @@ void MavlinkFtpTest::runTests(void)
 	ut_run_test(_removedirectory_test);
 	ut_run_test(_createdirectory_test);
 	ut_run_test(_removefile_test);
+	
+	return (_tests_failed == 0);
+
 }

+ut_declare_test(mavlink_ftp_test, MavlinkFtpTest)
@@ -46,10 +46,7 @@ public:
 	MavlinkFtpTest();
 	virtual ~MavlinkFtpTest();
 	
-	virtual void init(void);
-	virtual void cleanup(void);
-	
-	virtual void runTests(void);
+	virtual bool run_tests(void);
 	
 	static void receive_message(const mavlink_message_t *msg, MavlinkFtpTest* ftpTest);
 	
@@ -65,6 +62,9 @@ public:
 	MavlinkFtpTest& operator=(const MavlinkFtpTest&);
 	
 private:
+	virtual void _init(void);
+	virtual void _cleanup(void);
+	
 	bool _ack_test(void);
 	bool _bad_opcode_test(void);
 	bool _bad_datasize_test(void);
@@ -105,3 +105,4 @@ private:
 	static const char _unittest_microsd_file[];
 };

+bool mavlink_ftp_test(void);
@@ -43,10 +43,5 @@ extern "C" __EXPORT int mavlink_tests_main(int argc, char *argv[]);

 int mavlink_tests_main(int argc, char *argv[])
 {
-	MavlinkFtpTest* test = new MavlinkFtpTest;
-	
-	test->runTests();
-	test->printResults();
-
-	return 0;
+	return mavlink_ftp_test() ? 0 : -1;
 }
@@ -45,4 +45,6 @@ INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink

 MODULE_STACKSIZE = 5000

+MAXOPTIMIZATION	 = -Os
+
 EXTRACXXFLAGS	= -Weffc++ -DMAVLINK_FTP_UNIT_TEST
@@ -76,6 +76,7 @@

 #define TILT_COS_MAX	0.7f
 #define SIGMA			0.000001f
+#define MIN_DIST		0.01f

 /**
 * Multicopter position control app start / stop handling function
@@ -179,6 +180,7 @@ private:

 	bool _reset_pos_sp;
 	bool _reset_alt_sp;
+	bool _mode_auto;

 	math::Vector<3> _pos;
 	math::Vector<3> _pos_sp;
@@ -219,6 +221,11 @@ private:
 	 */
 	void		reset_alt_sp();

+	/**
+	 * Check if position setpoint is too far from current position and adjust it if needed.
+	 */
+	void		limit_pos_sp_offset();
+
 	/**
 	 * Set position setpoint using manual control
 	 */
@@ -229,6 +236,14 @@ private:
 	 */
 	void		control_offboard(float dt);

+	bool		cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
+					const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res);
+
+	/**
+	 * Set position setpoint for AUTO
+	 */
+	void		control_auto(float dt);
+
 	/**
 	 * Select between barometric and global (AMSL) altitudes
 	 */
@@ -283,7 +298,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_ref_timestamp(0),

 	_reset_pos_sp(true),
-	_reset_alt_sp(true)
+	_reset_alt_sp(true),
+	_mode_auto(false)
 {
 	memset(&_att, 0, sizeof(_att));
 	memset(&_att_sp, 0, sizeof(_att_sp));
@@ -533,6 +549,29 @@ MulticopterPositionControl::reset_alt_sp()
 	}
 }

+void
+MulticopterPositionControl::limit_pos_sp_offset()
+{
+	math::Vector<3> pos_sp_offs;
+	pos_sp_offs.zero();
+
+	if (_control_mode.flag_control_position_enabled) {
+		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	}
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	}
+
+	float pos_sp_offs_norm = pos_sp_offs.length();
+
+	if (pos_sp_offs_norm > 1.0f) {
+		pos_sp_offs /= pos_sp_offs_norm;
+		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+	}
+}
+
 void
 MulticopterPositionControl::control_manual(float dt)
 {
@@ -614,7 +653,6 @@ MulticopterPositionControl::control_offboard(float dt)
 			_pos_sp(0) = _pos_sp_triplet.current.x;
 			_pos_sp(1) = _pos_sp_triplet.current.y;
 			_pos_sp(2) = _pos_sp_triplet.current.z;
-			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;

 		} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
 			/* control velocity */
@@ -624,6 +662,11 @@ MulticopterPositionControl::control_offboard(float dt)
 			/* set position setpoint move rate */
 			_sp_move_rate(0) = _pos_sp_triplet.current.vx;
 			_sp_move_rate(1) = _pos_sp_triplet.current.vy;
+		}
+
+		if (_pos_sp_triplet.current.yaw_valid) {
+			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+		} else if (_pos_sp_triplet.current.yawspeed_valid) {
 			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
 		}

@@ -647,6 +690,170 @@ MulticopterPositionControl::control_offboard(float dt)
 	}
 }

+bool
+MulticopterPositionControl::cross_sphere_line(const math::Vector<3>& sphere_c, float sphere_r,
+		const math::Vector<3> line_a, const math::Vector<3> line_b, math::Vector<3>& res)
+{
+	/* project center of sphere on line */
+	/* normalized AB */
+	math::Vector<3> ab_norm = line_b - line_a;
+	ab_norm.normalize();
+	math::Vector<3> d = line_a + ab_norm * ((sphere_c - line_a) * ab_norm);
+	float cd_len = (sphere_c - d).length();
+
+	/* we have triangle CDX with known CD and CX = R, find DX */
+	if (sphere_r > cd_len) {
+		/* have two roots, select one in A->B direction from D */
+		float dx_len = sqrtf(sphere_r * sphere_r - cd_len * cd_len);
+		res = d + ab_norm * dx_len;
+		return true;
+
+	} else {
+		/* have no roots, return D */
+		res = d;
+		return false;
+	}
+}
+
+void
+MulticopterPositionControl::control_auto(float dt)
+{
+	if (!_mode_auto) {
+		_mode_auto = true;
+		/* reset position setpoint on AUTO mode activation */
+		reset_pos_sp();
+		reset_alt_sp();
+	}
+
+	bool updated;
+	orb_check(_pos_sp_triplet_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+	}
+
+	if (_pos_sp_triplet.current.valid) {
+		/* in case of interrupted mission don't go to waypoint but stay at current position */
+		_reset_pos_sp = true;
+		_reset_alt_sp = true;
+
+		/* project setpoint to local frame */
+		math::Vector<3> curr_sp;
+		map_projection_project(&_ref_pos,
+				       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
+				       &curr_sp.data[0], &curr_sp.data[1]);
+		curr_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
+
+		/* scaled space: 1 == position error resulting max allowed speed, L1 = 1 in this space */
+		math::Vector<3> scale = _params.pos_p.edivide(_params.vel_max);	// TODO add mult param here
+
+		/* convert current setpoint to scaled space */
+		math::Vector<3> curr_sp_s = curr_sp.emult(scale);
+
+		/* by default use current setpoint as is */
+		math::Vector<3> pos_sp_s = curr_sp_s;
+
+		if (_pos_sp_triplet.current.type == SETPOINT_TYPE_POSITION && _pos_sp_triplet.previous.valid) {
+			/* follow "previous - current" line */
+			math::Vector<3> prev_sp;
+			map_projection_project(&_ref_pos,
+						   _pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon,
+						   &prev_sp.data[0], &prev_sp.data[1]);
+			prev_sp(2) = -(_pos_sp_triplet.previous.alt - _ref_alt);
+
+			if ((curr_sp - prev_sp).length() > MIN_DIST) {
+
+				/* find X - cross point of L1 sphere and trajectory */
+				math::Vector<3> pos_s = _pos.emult(scale);
+				math::Vector<3> prev_sp_s = prev_sp.emult(scale);
+				math::Vector<3> prev_curr_s = curr_sp_s - prev_sp_s;
+				math::Vector<3> curr_pos_s = pos_s - curr_sp_s;
+				float curr_pos_s_len = curr_pos_s.length();
+				if (curr_pos_s_len < 1.0f) {
+					/* copter is closer to waypoint than L1 radius */
+					/* check next waypoint and use it to avoid slowing down when passing via waypoint */
+					if (_pos_sp_triplet.next.valid) {
+						math::Vector<3> next_sp;
+						map_projection_project(&_ref_pos,
+									   _pos_sp_triplet.next.lat, _pos_sp_triplet.next.lon,
+									   &next_sp.data[0], &next_sp.data[1]);
+						next_sp(2) = -(_pos_sp_triplet.next.alt - _ref_alt);
+
+						if ((next_sp - curr_sp).length() > MIN_DIST) {
+							math::Vector<3> next_sp_s = next_sp.emult(scale);
+
+							/* calculate angle prev - curr - next */
+							math::Vector<3> curr_next_s = next_sp_s - curr_sp_s;
+							math::Vector<3> prev_curr_s_norm = prev_curr_s.normalized();
+
+							/* cos(a) * curr_next, a = angle between current and next trajectory segments */
+							float cos_a_curr_next = prev_curr_s_norm * curr_next_s;
+
+							/* cos(b), b = angle pos - curr_sp - prev_sp */
+							float cos_b = -curr_pos_s * prev_curr_s_norm / curr_pos_s_len;
+
+							if (cos_a_curr_next > 0.0f && cos_b > 0.0f) {
+								float curr_next_s_len = curr_next_s.length();
+								/* if curr - next distance is larger than L1 radius, limit it */
+								if (curr_next_s_len > 1.0f) {
+									cos_a_curr_next /= curr_next_s_len;
+								}
+
+								/* feed forward position setpoint offset */
+								math::Vector<3> pos_ff = prev_curr_s_norm *
+										cos_a_curr_next * cos_b * cos_b * (1.0f - curr_pos_s_len) *
+										(1.0f - expf(-curr_pos_s_len * curr_pos_s_len * 20.0f));
+								pos_sp_s += pos_ff;
+							}
+						}
+					}
+
+				} else {
+					bool near = cross_sphere_line(pos_s, 1.0f, prev_sp_s, curr_sp_s, pos_sp_s);
+					if (near) {
+						/* L1 sphere crosses trajectory */
+
+					} else {
+						/* copter is too far from trajectory */
+						/* if copter is behind prev waypoint, go directly to prev waypoint */
+						if ((pos_sp_s - prev_sp_s) * prev_curr_s < 0.0f) {
+							pos_sp_s = prev_sp_s;
+						}
+
+						/* if copter is in front of curr waypoint, go directly to curr waypoint */
+						if ((pos_sp_s - curr_sp_s) * prev_curr_s > 0.0f) {
+							pos_sp_s = curr_sp_s;
+						}
+
+						pos_sp_s = pos_s + (pos_sp_s - pos_s).normalized();
+					}
+				}
+			}
+		}
+
+		/* move setpoint not faster than max allowed speed */
+		math::Vector<3> pos_sp_old_s = _pos_sp.emult(scale);
+
+		/* difference between current and desired position setpoints, 1 = max speed */
+		math::Vector<3> d_pos_m = (pos_sp_s - pos_sp_old_s).edivide(_params.pos_p);
+		float d_pos_m_len = d_pos_m.length();
+		if (d_pos_m_len > dt) {
+			pos_sp_s = pos_sp_old_s + (d_pos_m / d_pos_m_len * dt).emult(_params.pos_p);
+		}
+
+		/* scale result back to normal space */
+		_pos_sp = pos_sp_s.edivide(scale);
+
+		/* update yaw setpoint if needed */
+		if (isfinite(_pos_sp_triplet.current.yaw)) {
+			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+		}
+
+	} else {
+		/* no waypoint, do nothing, setpoint was already reset */
+	}
+}
+
 void
 MulticopterPositionControl::task_main()
 {
@@ -750,41 +957,16 @@ MulticopterPositionControl::task_main()
 			if (_control_mode.flag_control_manual_enabled) {
 				/* manual control */
 				control_manual(dt);
+				_mode_auto = false;

 			} else if (_control_mode.flag_control_offboard_enabled) {
 				/* offboard control */
 				control_offboard(dt);
+				_mode_auto = false;

 			} else {
 				/* AUTO */
-				bool updated;
-				orb_check(_pos_sp_triplet_sub, &updated);
-
-				if (updated) {
-					orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
-				}
-
-				if (_pos_sp_triplet.current.valid) {
-					/* in case of interrupted mission don't go to waypoint but stay at current position */
-					_reset_pos_sp = true;
-					_reset_alt_sp = true;
-
-					/* project setpoint to local frame */
-					map_projection_project(&_ref_pos,
-							       _pos_sp_triplet.current.lat, _pos_sp_triplet.current.lon,
-							       &_pos_sp.data[0], &_pos_sp.data[1]);
-					_pos_sp(2) = -(_pos_sp_triplet.current.alt - _ref_alt);
-
-					/* update yaw setpoint if needed */
-					if (isfinite(_pos_sp_triplet.current.yaw)) {
-						_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
-					}
-
-				} else {
-					/* no waypoint, loiter, reset position setpoint if needed */
-					reset_pos_sp();
-					reset_alt_sp();
-				}
+				control_auto(dt);
 			}

 			/* fill local position setpoint */
@@ -846,16 +1028,6 @@ MulticopterPositionControl::task_main()
 					_vel_sp(2) = _params.land_speed;
 				}

-
-				if (!_control_mode.flag_control_manual_enabled) {
-					/* limit 3D speed only in non-manual modes */
-					float vel_sp_norm = _vel_sp.edivide(_params.vel_max).length();
-
-					if (vel_sp_norm > 1.0f) {
-						_vel_sp /= vel_sp_norm;
-					}
-				}
-
 				_global_vel_sp.vx = _vel_sp(0);
 				_global_vel_sp.vy = _vel_sp(1);
 				_global_vel_sp.vz = _vel_sp(2);
@@ -1132,9 +1304,9 @@ MulticopterPositionControl::task_main()
 			/* position controller disabled, reset setpoints */
 			_reset_alt_sp = true;
 			_reset_pos_sp = true;
+			_mode_auto = false;
 			reset_int_z = true;
 			reset_int_xy = true;
-
 		}

 		/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
--- a/Show More
+++ b/Show More