updated from remote

ROMFS/px4fmu_common/init.d/rc.vtol_apps

@@ -0,0 +1,15 @@
+#!nsh
+#
+# Standard apps for vtol:
+# att & pos estimator, att & pos control.
+#
+
+attitude_estimator_ekf start
+#ekf_att_pos_estimator start
+position_estimator_inav start
+
+vtol_att_control start
+mc_att_control start
+mc_pos_control start
+fw_att_control start
+fw_pos_control_l1 start

ROMFS/px4fmu_common/init.d/rc.vtol_defaults

@@ -0,0 +1,63 @@
+#!nsh
+
+set VEHICLE_TYPE vtol
+
+if [ $DO_AUTOCONFIG == yes ]
+then
+	#Default parameters for MC
+	param set MC_ROLL_P 7.0
+	param set MC_ROLLRATE_P 0.1
+	param set MC_ROLLRATE_I 0.0
+	param set MC_ROLLRATE_D 0.003
+	param set MC_PITCH_P 7.0
+	param set MC_PITCHRATE_P 0.1
+	param set MC_PITCHRATE_I 0.0
+	param set MC_PITCHRATE_D 0.003
+	param set MC_YAW_P 2.8
+	param set MC_YAWRATE_P 0.2
+	param set MC_YAWRATE_I 0.1
+	param set MC_YAWRATE_D 0.0
+	param set MC_YAW_FF 0.5
+
+	param set MPC_THR_MAX 1.0
+	param set MPC_THR_MIN 0.1
+	param set MPC_XY_P 1.0
+	param set MPC_XY_VEL_P 0.1
+	param set MPC_XY_VEL_I 0.02
+	param set MPC_XY_VEL_D 0.01
+	param set MPC_XY_VEL_MAX 5
+	param set MPC_XY_FF 0.5
+	param set MPC_Z_P 1.0
+	param set MPC_Z_VEL_P 0.1
+	param set MPC_Z_VEL_I 0.02
+	param set MPC_Z_VEL_D 0.0
+	param set MPC_Z_VEL_MAX 3
+	param set MPC_Z_FF 0.5
+	param set MPC_TILTMAX_AIR 45.0
+	param set MPC_TILTMAX_LND 15.0
+	param set MPC_LAND_SPEED 1.0
+
+	param set PE_VELNE_NOISE 0.5
+	param set PE_VELD_NOISE 0.7
+	param set PE_POSNE_NOISE 0.5
+	param set PE_POSD_NOISE 1.0
+
+	param set NAV_ACCEPT_RAD 2.0
+	
+	#
+	# Default parameters for FW
+	#
+	param set NAV_LAND_ALT 90
+	param set NAV_RTL_ALT 100
+	param set NAV_RTL_LAND_T -1
+	param set NAV_ACCEPT_RAD 50
+	param set FW_T_HRATE_P 0.01
+	param set FW_T_RLL2THR 15
+	param set FW_T_SRATE_P 0.01
+	param set FW_T_TIME_CONST 5
+fi
+
+set PWM_RATE 400
+set PWM_DISARMED 900
+set PWM_MIN 1075
+set PWM_MAX 2000

ROMFS/px4fmu_common/mixers/FMU_quadshot.mix

@@ -0,0 +1,15 @@
+#!nsh
+#Quadshot mixer for PX4FMU
+#===========================
+R: 4v 10000 10000 10000 0
+
+#mixer for the elevons
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 0   7500   7500      0 -10000  10000
+S: 1 1   8000   8000      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 1 0   7500   7500      0 -10000  10000
+S: 1 1  -8000  -8000      0 -10000  10000

Tools/tests-host/sf0x_test.cpp

@@ -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;
+}

Tools/tests-host/st24_test.cpp

@@ -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);
+	}
+
+}

src/drivers/sf0x/sf0x_parser.cpp

@@ -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;
+}

src/drivers/sf0x/sf0x_parser.h

@@ -0,0 +1,51 @@
+/****************************************************************************
+ *
+ *   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>
+ *
+ * Declarations of parser for the Lightware SF0x laser rangefinder series
+ */
+
+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
+};
+
+int sf0x_parser(char c, char *parserbuf, unsigned *parserbuf_index, enum SF0X_PARSE_STATE *state, float *dist);

src/lib/rc/module.mk

@@ -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

src/lib/rc/st24.c

@@ -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;
+}

src/lib/rc/st24.h

@@ -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

src/modules/bottle_drop/bottle_drop.cpp

@@ -0,0 +1,907 @@
+/****************************************************************************
+ *
+ *   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.cpp
+ *
+ * Bottle drop module for Outback Challenge 2014, Team Swiss Fang
+ *
+ * @author Dominik Juchli <juchlid@ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <sys/ioctl.h>
+#include <drivers/device/device.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/wind_estimate.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+#include <dataman/dataman.h>
+#include <mathlib/mathlib.h>
+#include <mavlink/mavlink_log.h>
+
+
+/**
+ * bottle_drop app start / stop handling function
+ *
+ * @ingroup apps
+ */
+extern "C" __EXPORT int bottle_drop_main(int argc, char *argv[]);
+
+class BottleDrop
+{
+public:
+	/**
+	 * Constructor
+	 */
+	BottleDrop();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~BottleDrop();
+
+	/**
+	 * Start the task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+	/**
+	 * Display status.
+	 */
+	void		status();
+
+	void		open_bay();
+	void		close_bay();
+	void		drop();
+	void		lock_release();
+
+private:
+	bool		_task_should_exit;		/**< if true, task should exit */
+	int		_main_task;			/**< handle for task */
+	int		_mavlink_fd;
+
+	int		_command_sub;
+	int		_wind_estimate_sub;
+	struct vehicle_command_s	_command;
+	struct vehicle_global_position_s _global_pos;
+	map_projection_reference_s ref;
+
+	orb_advert_t	_actuator_pub;
+	struct actuator_controls_s _actuators;
+
+	bool		_drop_approval;
+	hrt_abstime	_doors_opened;
+	hrt_abstime	_drop_time;
+
+	float		_alt_clearance;
+
+	struct position_s {
+		double lat;	///< degrees
+		double lon;	///< degrees
+		float alt;	///< m
+	} _target_position, _drop_position;
+
+	enum DROP_STATE {
+		DROP_STATE_INIT = 0,
+		DROP_STATE_TARGET_VALID,
+		DROP_STATE_TARGET_SET,
+		DROP_STATE_BAY_OPEN,
+		DROP_STATE_DROPPED,
+		DROP_STATE_BAY_CLOSED
+	} _drop_state;
+
+	struct mission_s	_onboard_mission;
+	orb_advert_t		_onboard_mission_pub;
+
+	void		task_main();
+
+	void		handle_command(struct vehicle_command_s *cmd);
+
+	void		answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result);
+
+	/**
+	 * Set the actuators
+	 */
+	int		actuators_publish();
+
+	/**
+	 * Shim for calling task_main from task_create.
+	 */
+	static void	task_main_trampoline(int argc, char *argv[]);
+};
+
+namespace bottle_drop
+{
+BottleDrop	*g_bottle_drop;
+}
+
+BottleDrop::BottleDrop() :
+
+	_task_should_exit(false),
+	_main_task(-1),
+	_mavlink_fd(-1),
+	_command_sub(-1),
+	_wind_estimate_sub(-1),
+	_command {},
+	_global_pos {},
+	ref {},
+	_actuator_pub(-1),
+	_actuators {},
+	_drop_approval(false),
+	_doors_opened(0),
+	_drop_time(0),
+	_alt_clearance(70.0f),
+	_target_position {},
+	_drop_position {},
+	_drop_state(DROP_STATE_INIT),
+	_onboard_mission {},
+	_onboard_mission_pub(-1)
+{
+}
+
+BottleDrop::~BottleDrop()
+{
+	if (_main_task != -1) {
+
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_main_task);
+				break;
+			}
+		} while (_main_task != -1);
+	}
+
+	bottle_drop::g_bottle_drop = nullptr;
+}
+
+int
+BottleDrop::start()
+{
+	ASSERT(_main_task == -1);
+
+	/* start the task */
+	_main_task = task_spawn_cmd("bottle_drop",
+				    SCHED_DEFAULT,
+				    SCHED_PRIORITY_DEFAULT + 15,
+				    2048,
+				    (main_t)&BottleDrop::task_main_trampoline,
+				    nullptr);
+
+	if (_main_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+
+void
+BottleDrop::status()
+{
+	warnx("drop state: %d", _drop_state);
+}
+
+void
+BottleDrop::open_bay()
+{
+	_actuators.control[0] = -1.0f;
+	_actuators.control[1] = 1.0f;
+
+	if (_doors_opened == 0) {
+		_doors_opened = hrt_absolute_time();
+	}
+	warnx("open doors");
+
+	actuators_publish();
+
+	usleep(500 * 1000);
+}
+
+void
+BottleDrop::close_bay()
+{
+	// closed door and locked survival kit
+	_actuators.control[0] = 1.0f;
+	_actuators.control[1] = -1.0f;
+
+	_doors_opened = 0;
+
+	actuators_publish();
+
+	// delay until the bay is closed
+	usleep(500 * 1000);
+}
+
+void
+BottleDrop::drop()
+{
+
+	// update drop actuator, wait 0.5s until the doors are open before dropping
+	hrt_abstime starttime = hrt_absolute_time();
+
+	// force the door open if we have to
+	if (_doors_opened == 0) {
+		open_bay();
+		warnx("bay not ready, forced open");
+	}
+
+	while (hrt_elapsed_time(&_doors_opened) < 500 * 1000 && hrt_elapsed_time(&starttime) < 2000000) {
+		usleep(50000);
+		warnx("delayed by door!");
+	}
+
+	_actuators.control[2] = 1.0f;
+
+	_drop_time = hrt_absolute_time();
+	actuators_publish();
+
+	warnx("dropping now");
+
+	// Give it time to drop
+	usleep(1000 * 1000);
+}
+
+void
+BottleDrop::lock_release()
+{
+	_actuators.control[2] = -1.0f;
+	actuators_publish();
+
+	warnx("closing release");
+}
+
+int
+BottleDrop::actuators_publish()
+{
+	_actuators.timestamp = hrt_absolute_time();
+
+	// lazily publish _actuators only once available
+	if (_actuator_pub > 0) {
+		return orb_publish(ORB_ID(actuator_controls_2), _actuator_pub, &_actuators);
+
+	} else {
+		_actuator_pub = orb_advertise(ORB_ID(actuator_controls_2), &_actuators);
+		if (_actuator_pub > 0) {
+			return OK;
+		} else {
+			return -1;
+		}
+	}
+}
+
+void
+BottleDrop::task_main()
+{
+
+	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+	mavlink_log_info(_mavlink_fd, "[bottle_drop] started");
+
+	_command_sub = orb_subscribe(ORB_ID(vehicle_command));
+	_wind_estimate_sub = orb_subscribe(ORB_ID(wind_estimate));
+
+	bool updated = false;
+
+	float z_0;		// ground properties
+	float turn_radius;	// turn radius of the UAV
+	float precision;	// Expected precision of the UAV
+
+	float ground_distance = _alt_clearance;		// Replace by closer estimate in loop
+
+	// constant
+	float g = CONSTANTS_ONE_G;               		// constant of gravity [m/s^2]
+	float m = 0.5f;                		// mass of bottle [kg]
+	float rho = 1.2f;              		// air density [kg/m^3]
+	float A = ((0.063f * 0.063f) / 4.0f * M_PI_F); // Bottle cross section [m^2]
+	float dt_freefall_prediction = 0.01f;   // step size of the free fall prediction [s]
+
+	// Has to be estimated by experiment
+	float cd = 0.86f;              	// Drag coefficient for a cylinder with a d/l ratio of 1/3 []
+	float t_signal =
+		0.084f;	// Time span between sending the signal and the bottle top reaching level height with the bottom of the plane [s]
+	float t_door =
+		0.7f;		// The time the system needs to open the door + safety, is also the time the palyload needs to safely escape the shaft [s]
+
+
+	// Definition
+	float h_0;						// height over target
+	float az;                 				// acceleration in z direction[m/s^2]
+	float vz; 						// velocity in z direction [m/s]
+	float z; 						// fallen distance [m]
+	float h; 						// height over target [m]
+	float ax;						// acceleration in x direction [m/s^2]
+	float vx;						// ground speed in x direction [m/s]
+	float x;					        // traveled distance in x direction [m]
+	float vw;                 				// wind speed [m/s]
+	float vrx;						// relative velocity in x direction [m/s]
+	float v;						// relative speed vector [m/s]
+	float Fd;						// Drag force [N]
+	float Fdx;						// Drag force in x direction [N]
+	float Fdz;						// Drag force in z direction [N]
+	float x_drop, y_drop;					// coordinates of the drop point in reference to the target (projection of NED)
+	float x_t, y_t;						// coordinates of the target in reference to the target x_t = 0, y_t = 0 (projection of NED)
+	float x_l, y_l;						// local position in projected coordinates
+	float x_f, y_f;						// to-be position of the UAV after dt_runs seconds in projected coordinates
+	double x_f_NED, y_f_NED;				// to-be position of the UAV after dt_runs seconds in NED
+	float distance_open_door;				// The distance the UAV travels during its doors open [m]
+	float approach_error = 0.0f;				// The error in radians between current ground vector and desired ground vector
+	float distance_real = 0;				// The distance between the UAVs position and the drop point [m]
+	float future_distance = 0;				// The distance between the UAVs to-be position and the drop point [m]
+
+	unsigned counter = 0;
+
+	param_t param_gproperties = param_find("BD_GPROPERTIES");
+	param_t param_turn_radius = param_find("BD_TURNRADIUS");
+	param_t param_precision = param_find("BD_PRECISION");
+	param_t param_cd = param_find("BD_OBJ_CD");
+	param_t param_mass = param_find("BD_OBJ_MASS");
+	param_t param_surface = param_find("BD_OBJ_SURFACE");
+
+
+	param_get(param_precision, &precision);
+	param_get(param_turn_radius, &turn_radius);
+	param_get(param_gproperties, &z_0);
+	param_get(param_cd, &cd);
+	param_get(param_mass, &m);
+	param_get(param_surface, &A);
+
+	int vehicle_global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+
+	struct parameter_update_s update;
+	memset(&update, 0, sizeof(update));
+	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
+
+	struct mission_item_s flight_vector_s {};
+	struct mission_item_s flight_vector_e {};
+
+	flight_vector_s.nav_cmd = NAV_CMD_WAYPOINT;
+	flight_vector_s.acceptance_radius = 50; // TODO: make parameter
+	flight_vector_s.autocontinue = true;
+	flight_vector_s.altitude_is_relative = false;
+
+	flight_vector_e.nav_cmd = NAV_CMD_WAYPOINT;
+	flight_vector_e.acceptance_radius = 50; // TODO: make parameter
+	flight_vector_e.autocontinue = true;
+	flight_vector_s.altitude_is_relative = false;
+
+	struct wind_estimate_s wind;
+
+	// wakeup source(s)
+	struct pollfd fds[1];
+
+	// Setup of loop
+	fds[0].fd = _command_sub;
+	fds[0].events = POLLIN;
+
+	// Whatever state the bay is in, we want it closed on startup
+	lock_release();
+	close_bay();
+
+	while (!_task_should_exit) {
+
+		/* wait for up to 100ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 50);
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			continue;
+		}
+
+		/* vehicle commands updated */
+		if (fds[0].revents & POLLIN) {
+			orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
+			handle_command(&_command);
+		}
+
+		orb_check(vehicle_global_position_sub, &updated);
+		if (updated) {
+			/* copy global position */
+			orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
+		}
+
+		if (_global_pos.timestamp == 0) {
+			continue;
+		}
+
+		const unsigned sleeptime_us = 9500;
+
+		hrt_abstime last_run = hrt_absolute_time();
+		float dt_runs = sleeptime_us / 1e6f;
+
+		// switch to faster updates during the drop
+		while (_drop_state > DROP_STATE_INIT) {
+
+			// Get wind estimate
+			orb_check(_wind_estimate_sub, &updated);
+			if (updated) {
+				orb_copy(ORB_ID(wind_estimate), _wind_estimate_sub, &wind);
+			}
+
+			// Get vehicle position
+			orb_check(vehicle_global_position_sub, &updated);
+			if (updated) {
+				// copy global position
+				orb_copy(ORB_ID(vehicle_global_position), vehicle_global_position_sub, &_global_pos);
+			}
+
+			// Get parameter updates
+			orb_check(parameter_update_sub, &updated);
+			if (updated) {
+				// copy global position
+				orb_copy(ORB_ID(parameter_update), parameter_update_sub, &update);
+
+				// update all parameters
+				param_get(param_gproperties, &z_0);
+				param_get(param_turn_radius, &turn_radius);
+				param_get(param_precision, &precision);
+			}
+
+			orb_check(_command_sub, &updated);
+			if (updated) {
+				orb_copy(ORB_ID(vehicle_command), _command_sub, &_command);
+				handle_command(&_command);
+			}
+
+
+			float windspeed_norm = sqrtf(wind.windspeed_north * wind.windspeed_north + wind.windspeed_east * wind.windspeed_east);
+			float groundspeed_body = sqrtf(_global_pos.vel_n * _global_pos.vel_n + _global_pos.vel_e * _global_pos.vel_e);
+			ground_distance = _global_pos.alt - _target_position.alt;
+
+			// Distance to drop position and angle error to approach vector
+			// are relevant in all states greater than target valid (which calculates these positions)
+			if (_drop_state > DROP_STATE_TARGET_VALID) {
+				distance_real = fabsf(get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, _drop_position.lat, _drop_position.lon));
+
+				float ground_direction = atan2f(_global_pos.vel_e, _global_pos.vel_n);
+				float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
+
+				approach_error = _wrap_pi(ground_direction - approach_direction);
+
+				if (counter % 90 == 0) {
+					mavlink_log_info(_mavlink_fd, "drop distance %u, heading error %u", (unsigned)distance_real, (unsigned)math::degrees(approach_error));
+				}
+			}
+
+			switch (_drop_state) {
+
+				case DROP_STATE_TARGET_VALID:
+				{
+
+					az = g;							// acceleration in z direction[m/s^2]
+					vz = 0; 						// velocity in z direction [m/s]
+					z = 0; 							// fallen distance [m]
+					h_0 = _global_pos.alt - _target_position.alt; 		// height over target at start[m]
+					h = h_0;						// height over target [m]
+					ax = 0;							// acceleration in x direction [m/s^2]
+					vx = groundspeed_body;// XXX project					// ground speed in x direction [m/s]
+					x = 0;							// traveled distance in x direction [m]
+					vw = 0;							// wind speed [m/s]
+					vrx = 0;						// relative velocity in x direction [m/s]
+					v = groundspeed_body;					// relative speed vector [m/s]
+					Fd = 0;							// Drag force [N]
+					Fdx = 0;						// Drag force in x direction [N]
+					Fdz = 0;						// Drag force in z direction [N]
+
+
+					// Compute the distance the bottle will travel after it is dropped in body frame coordinates --> x
+					while (h > 0.05f) {
+						// z-direction
+						vz = vz + az * dt_freefall_prediction;
+						z = z + vz * dt_freefall_prediction;
+						h = h_0 - z;
+
+						// x-direction
+						vw = windspeed_norm * logf(h / z_0) / logf(ground_distance / z_0);
+						vx = vx + ax * dt_freefall_prediction;
+						x = x + vx * dt_freefall_prediction;
+						vrx = vx + vw;
+
+						// drag force
+						v = sqrtf(vz * vz + vrx * vrx);
+						Fd = 0.5f * rho * A * cd * (v * v);
+						Fdx = Fd * vrx / v;
+						Fdz = Fd * vz / v;
+
+						// acceleration
+						az = g - Fdz / m;
+						ax = -Fdx / m;
+					}
+
+					// compute drop vector
+					x = groundspeed_body * t_signal + x;
+
+					x_t = 0.0f;
+					y_t = 0.0f;
+
+					float wind_direction_n, wind_direction_e;
+
+					if (windspeed_norm < 0.5f) {	// If there is no wind, an arbitrarily direction is chosen
+						wind_direction_n = 1.0f;
+						wind_direction_e = 0.0f;
+
+					} else {
+						wind_direction_n = wind.windspeed_north / windspeed_norm;
+						wind_direction_e = wind.windspeed_east / windspeed_norm;
+					}
+
+					x_drop = x_t + x * wind_direction_n;
+					y_drop = y_t + x * wind_direction_e;
+					map_projection_reproject(&ref, x_drop, y_drop, &_drop_position.lat, &_drop_position.lon);
+					_drop_position.alt = _target_position.alt + _alt_clearance;
+
+					// Compute flight vector
+					map_projection_reproject(&ref, x_drop + 2 * turn_radius * wind_direction_n, y_drop + 2 * turn_radius * wind_direction_e,
+								 &(flight_vector_s.lat), &(flight_vector_s.lon));
+					flight_vector_s.altitude = _drop_position.alt;
+					map_projection_reproject(&ref, x_drop - turn_radius * wind_direction_n, y_drop - turn_radius * wind_direction_e,
+								 &flight_vector_e.lat, &flight_vector_e.lon);
+					flight_vector_e.altitude = _drop_position.alt;
+
+					// Save WPs in datamanager
+					const ssize_t len = sizeof(struct mission_item_s);
+
+					if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 0, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_s, len) != len) {
+						warnx("ERROR: could not save onboard WP");
+					}
+
+					if (dm_write(DM_KEY_WAYPOINTS_ONBOARD, 1, DM_PERSIST_IN_FLIGHT_RESET, &flight_vector_e, len) != len) {
+						warnx("ERROR: could not save onboard WP");
+					}
+
+					_onboard_mission.count = 2;
+					_onboard_mission.current_seq = 0;
+
+					if (_onboard_mission_pub > 0) {
+						orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+
+					} else {
+						_onboard_mission_pub = orb_advertise(ORB_ID(onboard_mission), &_onboard_mission);
+					}
+
+					float approach_direction = get_bearing_to_next_waypoint(flight_vector_s.lat, flight_vector_s.lon, flight_vector_e.lat, flight_vector_e.lon);
+					mavlink_log_critical(_mavlink_fd, "position set, approach heading: %u", (unsigned)distance_real, (unsigned)math::degrees(approach_direction + M_PI_F));
+
+					_drop_state = DROP_STATE_TARGET_SET;
+				}
+				break;
+
+				case DROP_STATE_TARGET_SET:
+				{
+					float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
+
+					if (distance_wp2 < distance_real) {
+						_onboard_mission.current_seq = 0;
+						orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+					} else {
+
+						// We're close enough - open the bay
+						distance_open_door = math::max(10.0f, 3.0f * fabsf(t_door * groundspeed_body));
+
+						if (isfinite(distance_real) && distance_real < distance_open_door &&
+							fabsf(approach_error) < math::radians(20.0f)) {
+							open_bay();
+							_drop_state = DROP_STATE_BAY_OPEN;
+							mavlink_log_info(_mavlink_fd, "#audio: opening bay");
+						}
+					}
+				}
+				break;
+
+				case DROP_STATE_BAY_OPEN:
+					{
+						if (_drop_approval) {
+							map_projection_project(&ref, _global_pos.lat, _global_pos.lon, &x_l, &y_l);
+							x_f = x_l + _global_pos.vel_n * dt_runs;
+							y_f = y_l + _global_pos.vel_e * dt_runs;
+							map_projection_reproject(&ref, x_f, y_f, &x_f_NED, &y_f_NED);
+							future_distance = get_distance_to_next_waypoint(x_f_NED, y_f_NED, _drop_position.lat, _drop_position.lon);
+
+							if (isfinite(distance_real) &&
+								(distance_real < precision) && ((distance_real < future_distance))) {
+									drop();
+									_drop_state = DROP_STATE_DROPPED;
+									mavlink_log_info(_mavlink_fd, "#audio: payload dropped");
+							} else {
+
+								float distance_wp2 = get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon, flight_vector_e.lat, flight_vector_e.lon);
+
+								if (distance_wp2 < distance_real) {
+									_onboard_mission.current_seq = 0;
+									orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+								}
+							}
+						}
+					}
+					break;
+
+				case DROP_STATE_DROPPED:
+					/* 2s after drop, reset and close everything again */
+					if ((hrt_elapsed_time(&_doors_opened) > 2 * 1000 * 1000)) {
+						_drop_state = DROP_STATE_INIT;
+						_drop_approval = false;
+						lock_release();
+						close_bay();
+						mavlink_log_info(_mavlink_fd, "#audio: closing bay");
+
+						// remove onboard mission
+						_onboard_mission.current_seq = -1;
+						_onboard_mission.count = 0;
+						orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+					}
+					break;
+			}
+
+			counter++;
+
+			// update_actuators();
+
+			// run at roughly 100 Hz
+			usleep(sleeptime_us);
+
+			dt_runs = hrt_elapsed_time(&last_run) / 1e6f;
+			last_run = hrt_absolute_time();
+		}
+	}
+
+	warnx("exiting.");
+
+	_main_task = -1;
+	_exit(0);
+}
+
+void
+BottleDrop::handle_command(struct vehicle_command_s *cmd)
+{
+	switch (cmd->command) {
+	case VEHICLE_CMD_CUSTOM_0:
+		/*
+		 * param1 and param2 set to 1: open and drop
+		 * param1 set to 1: open
+		 * else: close (and don't drop)
+		 */
+		if (cmd->param1 > 0.5f && cmd->param2 > 0.5f) {
+			open_bay();
+			drop();
+			mavlink_log_info(_mavlink_fd, "#audio: drop bottle");
+
+		} else if (cmd->param1 > 0.5f) {
+			open_bay();
+			mavlink_log_info(_mavlink_fd, "#audio: opening bay");
+
+		} else {
+			lock_release();
+			close_bay();
+			mavlink_log_info(_mavlink_fd, "#audio: closing bay");
+		}
+
+		answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+		break;
+
+	case VEHICLE_CMD_PAYLOAD_PREPARE_DEPLOY:
+
+		switch ((int)(cmd->param1 + 0.5f)) {
+		case 0:
+			_drop_approval = false;
+			mavlink_log_info(_mavlink_fd, "#audio: got drop position, no approval");
+			break;
+
+		case 1:
+			_drop_approval = true;
+			mavlink_log_info(_mavlink_fd, "#audio: got drop position and approval");
+			break;
+
+		default:
+			_drop_approval = false;
+			warnx("param1 val unknown");
+			break;
+		}
+
+		// XXX check all fields (2-3)
+		_alt_clearance = cmd->param4;
+		_target_position.lat = cmd->param5;
+		_target_position.lon = cmd->param6;
+		_target_position.alt = cmd->param7;
+		_drop_state = DROP_STATE_TARGET_VALID;
+		mavlink_log_info(_mavlink_fd, "got target: %8.4f, %8.4f, %8.4f", (double)_target_position.lat,
+			(double)_target_position.lon, (double)_target_position.alt);
+		map_projection_init(&ref, _target_position.lat, _target_position.lon);
+		answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+		break;
+
+	case VEHICLE_CMD_PAYLOAD_CONTROL_DEPLOY:
+
+		if (cmd->param1 < 0) {
+
+			// Clear internal states
+			_drop_approval = false;
+			_drop_state = DROP_STATE_INIT;
+
+			// Abort if mission is present
+			_onboard_mission.current_seq = -1;
+
+			if (_onboard_mission_pub > 0) {
+				orb_publish(ORB_ID(onboard_mission), _onboard_mission_pub, &_onboard_mission);
+			}
+
+		} else {
+			switch ((int)(cmd->param1 + 0.5f)) {
+			case 0:
+				_drop_approval = false;
+				break;
+
+			case 1:
+				_drop_approval = true;
+				mavlink_log_info(_mavlink_fd, "#audio: got drop approval");
+				break;
+
+			default:
+				_drop_approval = false;
+				break;
+				// XXX handle other values
+			}
+		}
+
+		answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+BottleDrop::answer_command(struct vehicle_command_s *cmd, enum VEHICLE_CMD_RESULT result)
+{
+	switch (result) {
+	case VEHICLE_CMD_RESULT_ACCEPTED:
+		break;
+
+	case VEHICLE_CMD_RESULT_DENIED:
+		mavlink_log_critical(_mavlink_fd, "#audio: command denied: %u", cmd->command);
+		break;
+
+	case VEHICLE_CMD_RESULT_FAILED:
+		mavlink_log_critical(_mavlink_fd, "#audio: command failed: %u", cmd->command);
+		break;
+
+	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
+		mavlink_log_critical(_mavlink_fd, "#audio: command temporarily rejected: %u", cmd->command);
+		break;
+
+	case VEHICLE_CMD_RESULT_UNSUPPORTED:
+		mavlink_log_critical(_mavlink_fd, "#audio: command unsupported: %u", cmd->command);
+		break;
+
+	default:
+		break;
+	}
+}
+
+void
+BottleDrop::task_main_trampoline(int argc, char *argv[])
+{
+	bottle_drop::g_bottle_drop->task_main();
+}
+
+static void usage()
+{
+	errx(1, "usage: bottle_drop {start|stop|status}");
+}
+
+int bottle_drop_main(int argc, char *argv[])
+{
+	if (argc < 2) {
+		usage();
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (bottle_drop::g_bottle_drop != nullptr) {
+			errx(1, "already running");
+		}
+
+		bottle_drop::g_bottle_drop = new BottleDrop;
+
+		if (bottle_drop::g_bottle_drop == nullptr) {
+			errx(1, "alloc failed");
+		}
+
+		if (OK != bottle_drop::g_bottle_drop->start()) {
+			delete bottle_drop::g_bottle_drop;
+			bottle_drop::g_bottle_drop = nullptr;
+			err(1, "start failed");
+		}
+
+		return 0;
+	}
+
+	if (bottle_drop::g_bottle_drop == nullptr) {
+		errx(1, "not running");
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		delete bottle_drop::g_bottle_drop;
+		bottle_drop::g_bottle_drop = nullptr;
+
+	} else if (!strcmp(argv[1], "status")) {
+		bottle_drop::g_bottle_drop->status();
+
+	} else if (!strcmp(argv[1], "drop")) {
+		bottle_drop::g_bottle_drop->drop();
+
+	} else if (!strcmp(argv[1], "open")) {
+		bottle_drop::g_bottle_drop->open_bay();
+
+	} else if (!strcmp(argv[1], "close")) {
+		bottle_drop::g_bottle_drop->close_bay();
+
+	} else if (!strcmp(argv[1], "lock")) {
+		bottle_drop::g_bottle_drop->lock_release();
+
+	} else {
+		usage();
+	}
+
+	return 0;
+}

src/modules/bottle_drop/bottle_drop_params.c

@@ -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);

src/modules/bottle_drop/module.mk

@@ -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

src/modules/mc_att_control/mc_att_control_base.cpp

@@ -0,0 +1,283 @@
+/*
+ * mc_att_control_base.cpp
+ *
+ *  Created on: Sep 25, 2014
+ *      Author: roman
+ */
+
+#include "mc_att_control_base.h"
+#include <geo/geo.h>
+#include <math.h>
+
+#ifdef CONFIG_ARCH_ARM
+#else
+#include <cmath>
+using namespace std;
+#endif
+
+MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
+
+		_task_should_exit(false), _control_task(-1),
+
+		_actuators_0_circuit_breaker_enabled(false),
+
+		/* performance counters */
+		_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+
+{
+	memset(&_v_att, 0, sizeof(_v_att));
+	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
+	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_actuators, 0, sizeof(_actuators));
+	memset(&_armed, 0, sizeof(_armed));
+
+	_params.att_p.zero();
+	_params.rate_p.zero();
+	_params.rate_i.zero();
+	_params.rate_d.zero();
+	_params.yaw_ff = 0.0f;
+	_params.yaw_rate_max = 0.0f;
+	_params.man_roll_max = 0.0f;
+	_params.man_pitch_max = 0.0f;
+	_params.man_yaw_max = 0.0f;
+	_params.acro_rate_max.zero();
+
+	_rates_prev.zero();
+	_rates_sp.zero();
+	_rates_int.zero();
+	_thrust_sp = 0.0f;
+	_att_control.zero();
+
+	_I.identity();
+}
+
+MulticopterAttitudeControlBase::~MulticopterAttitudeControlBase() {
+}
+
+void MulticopterAttitudeControlBase::vehicle_attitude_setpoint_poll() {
+}
+
+void MulticopterAttitudeControlBase::control_attitude(float dt) {
+	float yaw_sp_move_rate = 0.0f;
+	bool publish_att_sp = false;
+
+	if (_v_control_mode.flag_control_manual_enabled) {
+		/* manual input, set or modify attitude setpoint */
+
+		if (_v_control_mode.flag_control_velocity_enabled
+				|| _v_control_mode.flag_control_climb_rate_enabled) {
+			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
+			vehicle_attitude_setpoint_poll();
+		}
+
+		if (!_v_control_mode.flag_control_climb_rate_enabled) {
+			/* pass throttle directly if not in altitude stabilized mode */
+			_v_att_sp.thrust = _manual_control_sp.z;
+			publish_att_sp = true;
+		}
+
+		if (!_armed.armed) {
+			/* reset yaw setpoint when disarmed */
+			_reset_yaw_sp = true;
+		}
+
+		/* move yaw setpoint in all modes */
+		if (_v_att_sp.thrust < 0.1f) {
+			// TODO
+			//if (_status.condition_landed) {
+			/* reset yaw setpoint if on ground */
+			//	reset_yaw_sp = true;
+			//}
+		} else {
+			/* move yaw setpoint */
+			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
+			_v_att_sp.yaw_body = _wrap_pi(
+					_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
+			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
+			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
+			if (yaw_offs < -yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
+
+			} else if (yaw_offs > yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
+			}
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+		/* reset yaw setpint to current position if needed */
+		if (_reset_yaw_sp) {
+			_reset_yaw_sp = false;
+			_v_att_sp.yaw_body = _v_att.yaw;
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+		if (!_v_control_mode.flag_control_velocity_enabled) {
+			/* update attitude setpoint if not in position control mode */
+			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
+			_v_att_sp.pitch_body = -_manual_control_sp.x
+					* _params.man_pitch_max;
+			_v_att_sp.R_valid = false;
+			publish_att_sp = true;
+		}
+
+	} else {
+		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
+		vehicle_attitude_setpoint_poll();
+
+		/* reset yaw setpoint after non-manual control mode */
+		_reset_yaw_sp = true;
+	}
+
+	_thrust_sp = _v_att_sp.thrust;
+
+	/* construct attitude setpoint rotation matrix */
+	math::Matrix<3, 3> R_sp;
+
+	if (_v_att_sp.R_valid) {
+		/* rotation matrix in _att_sp is valid, use it */
+		R_sp.set(&_v_att_sp.R_body[0][0]);
+
+	} else {
+		/* rotation matrix in _att_sp is not valid, use euler angles instead */
+		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body,
+				_v_att_sp.yaw_body);
+
+		/* copy rotation matrix back to setpoint struct */
+		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0],
+				sizeof(_v_att_sp.R_body));
+		_v_att_sp.R_valid = true;
+	}
+
+//	/* publish the attitude setpoint if needed */
+//	if (publish_att_sp) {
+//		_v_att_sp.timestamp = hrt_absolute_time();
+//
+//		if (_att_sp_pub > 0) {
+//			orb_publish(ORB_ID(vehicle_attitude_setpoint), _att_sp_pub,
+//					&_v_att_sp);
+//
+//		} else {
+//			_att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint),
+//					&_v_att_sp);
+//		}
+//	}
+
+	/* rotation matrix for current state */
+	math::Matrix<3, 3> R;
+	R.set(_v_att.R);
+
+	/* all input data is ready, run controller itself */
+
+	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
+	math::Vector < 3 > R_z(R(0, 2), R(1, 2), R(2, 2));
+	math::Vector < 3 > R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
+
+	/* axis and sin(angle) of desired rotation */
+	math::Vector < 3 > e_R = R.transposed() * (R_z % R_sp_z);
+
+	/* calculate angle error */
+	float e_R_z_sin = e_R.length();
+	float e_R_z_cos = R_z * R_sp_z;
+
+	/* calculate weight for yaw control */
+	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
+
+	/* calculate rotation matrix after roll/pitch only rotation */
+	math::Matrix<3, 3> R_rp;
+
+	if (e_R_z_sin > 0.0f) {
+		/* get axis-angle representation */
+		float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);
+		math::Vector < 3 > e_R_z_axis = e_R / e_R_z_sin;
+
+		e_R = e_R_z_axis * e_R_z_angle;
+
+		/* cross product matrix for e_R_axis */
+		math::Matrix<3, 3> e_R_cp;
+		e_R_cp.zero();
+		e_R_cp(0, 1) = -e_R_z_axis(2);
+		e_R_cp(0, 2) = e_R_z_axis(1);
+		e_R_cp(1, 0) = e_R_z_axis(2);
+		e_R_cp(1, 2) = -e_R_z_axis(0);
+		e_R_cp(2, 0) = -e_R_z_axis(1);
+		e_R_cp(2, 1) = e_R_z_axis(0);
+
+		/* rotation matrix for roll/pitch only rotation */
+		R_rp = R
+				* (_I + e_R_cp * e_R_z_sin
+						+ e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+
+	} else {
+		/* zero roll/pitch rotation */
+		R_rp = R;
+	}
+
+	/* R_rp and R_sp has the same Z axis, calculate yaw error */
+	math::Vector < 3 > R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
+	math::Vector < 3 > R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
+	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
+
+	if (e_R_z_cos < 0.0f) {
+		/* for large thrust vector rotations use another rotation method:
+		 * calculate angle and axis for R -> R_sp rotation directly */
+		math::Quaternion q;
+		q.from_dcm(R.transposed() * R_sp);
+		math::Vector < 3 > e_R_d = q.imag();
+		e_R_d.normalize();
+		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
+
+		/* use fusion of Z axis based rotation and direct rotation */
+		float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
+		e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;
+	}
+
+	/* calculate angular rates setpoint */
+	_rates_sp = _params.att_p.emult(e_R);
+
+	/* limit yaw rate */
+	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max,
+			_params.yaw_rate_max);
+
+	/* feed forward yaw setpoint rate */
+	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
+}
+
+void MulticopterAttitudeControlBase::control_attitude_rates(float dt) {
+	/* reset integral if disarmed */
+	if (!_armed.armed) {
+		_rates_int.zero();
+	}
+
+	/* current body angular rates */
+	math::Vector < 3 > rates;
+	rates(0) = _v_att.rollspeed;
+	rates(1) = _v_att.pitchspeed;
+	rates(2) = _v_att.yawspeed;
+
+	/* angular rates error */
+	math::Vector < 3 > rates_err = _rates_sp - rates;
+	_att_control = _params.rate_p.emult(rates_err)
+			+ _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int;
+	_rates_prev = rates;
+
+	/* update integral only if not saturated on low limit */
+	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
+		for (int i = 0; i < 3; i++) {
+			if (fabsf(_att_control(i)) < _thrust_sp) {
+				float rate_i = _rates_int(i)
+						+ _params.rate_i(i) * rates_err(i) * dt;
+
+				if (isfinite(
+						rate_i) && rate_i > -RATES_I_LIMIT && rate_i < RATES_I_LIMIT &&
+						_att_control(i) > -RATES_I_LIMIT && _att_control(i) < RATES_I_LIMIT) {
+					_rates_int(i) = rate_i;
+				}
+			}
+		}
+	}
+
+}

src/modules/mc_att_control/mc_att_control_base.h

@@ -0,0 +1,172 @@
+/*
+ * mc_att_control_base.h
+ *
+ *  Created on: Sep 25, 2014
+ *      Author: roman
+ */
+
+#ifndef MC_ATT_CONTROL_BASE_H_
+#define MC_ATT_CONTROL_BASE_H_
+
+/* 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 mc_att_control_main.cpp
+ * Multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
+ * The controller has two loops: P loop for angular error and PD loop for angular rate error.
+ * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
+ * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
+ * so actual rotation axis is not constant. For large deviations controller rotates copter around fixed axis.
+ * These two approaches fused seamlessly with weight depending on angular error.
+ * When thrust vector directed near-horizontally (e.g. roll ~= PI/2) yaw setpoint ignored because of singularity.
+ * Controller doesn't use Euler angles for work, they generated only for more human-friendly control and logging.
+ * If rotation matrix setpoint is invalid it will be generated from Euler angles for compatibility with old position controllers.
+ */
+
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/actuator_armed.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+//#include <systemlib/systemlib.h>
+#include <lib/mathlib/mathlib.h>
+
+
+/**
+ * Multicopter attitude control app start / stop handling function
+ *
+ * @ingroup apps
+ */
+
+#define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
+#define RATES_I_LIMIT	0.3f
+
+class MulticopterAttitudeControlBase {
+public:
+	/**
+	 * Constructor
+	 */
+	MulticopterAttitudeControlBase();
+
+	/**
+	 * Destructor, also kills the sensors task.
+	 */
+	~MulticopterAttitudeControlBase();
+
+	/**
+	 * Start the sensors task.
+	 *
+	 * @return		OK on success.
+	 */
+
+protected:
+
+	bool _task_should_exit; /**< if true, sensor task should exit */
+	int _control_task; /**< task handle for sensor task */
+
+
+
+
+	bool _actuators_0_circuit_breaker_enabled; /**< circuit breaker to suppress output */
+
+	struct vehicle_attitude_s _v_att; /**< vehicle attitude */
+	struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
+	struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
+	struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
+	struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
+	struct actuator_controls_s _actuators; /**< actuator controls */
+	struct actuator_armed_s _armed; /**< actuator arming status */
+
+	perf_counter_t _loop_perf; /**< loop performance counter */
+
+	math::Vector<3> _rates_prev; /**< angular rates on previous step */
+	math::Vector<3> _rates_sp; /**< angular rates setpoint */
+	math::Vector<3> _rates_int; /**< angular rates integral error */
+	float _thrust_sp; /**< thrust setpoint */
+	math::Vector<3> _att_control; /**< attitude control vector */
+
+	math::Matrix<3, 3> _I; /**< identity matrix */
+
+	bool _reset_yaw_sp; /**< reset yaw setpoint flag */
+
+
+
+	struct {
+		math::Vector<3> att_p; /**< P gain for angular error */
+		math::Vector<3> rate_p; /**< P gain for angular rate error */
+		math::Vector<3> rate_i; /**< I gain for angular rate error */
+		math::Vector<3> rate_d; /**< D gain for angular rate error */
+		float yaw_ff; /**< yaw control feed-forward */
+		float yaw_rate_max; /**< max yaw rate */
+
+		float man_roll_max;
+		float man_pitch_max;
+		float man_yaw_max;
+		math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
+	} _params;
+
+
+	/**
+	 * Attitude controller.
+	 */
+	//void control_attitude(float dt);
+
+	/**
+	 * Attitude rates controller.
+	 */
+	void control_attitude(float dt);
+	void control_attitude_rates(float dt);
+
+	void vehicle_attitude_setpoint_poll();	//provisional
+
+
+};
+
+#endif /* MC_ATT_CONTROL_BASE_H_ */

src/modules/navigator/datalinkloss.cpp

@@ -0,0 +1,227 @@
+/****************************************************************************
+ *
+ *   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 datalinkloss.cpp
+ * Helper class for Data Link Loss Mode according to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "navigator.h"
+#include "datalinkloss.h"
+
+#define DELAY_SIGMA	0.01f
+
+DataLinkLoss::DataLinkLoss(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_param_commsholdwaittime(this, "CH_T"),
+	_param_commsholdlat(this, "CH_LAT"),
+	_param_commsholdlon(this, "CH_LON"),
+	_param_commsholdalt(this, "CH_ALT"),
+	_param_airfieldhomelat(this, "NAV_AH_LAT", false),
+	_param_airfieldhomelon(this, "NAV_AH_LON", false),
+	_param_airfieldhomealt(this, "NAV_AH_ALT", false),
+	_param_airfieldhomewaittime(this, "AH_T"),
+	_param_numberdatalinklosses(this, "N"),
+	_param_skipcommshold(this, "CHSK"),
+	_dll_state(DLL_STATE_NONE)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+DataLinkLoss::~DataLinkLoss()
+{
+}
+
+void
+DataLinkLoss::on_inactive()
+{
+	/* reset DLL state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+		_dll_state = DLL_STATE_NONE;
+	}
+}
+
+void
+DataLinkLoss::on_activation()
+{
+	_dll_state = DLL_STATE_NONE;
+	updateParams();
+	advance_dll();
+	set_dll_item();
+}
+
+void
+DataLinkLoss::on_active()
+{
+	if (is_mission_item_reached()) {
+		updateParams();
+		advance_dll();
+		set_dll_item();
+	}
+}
+
+void
+DataLinkLoss::set_dll_item()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	set_previous_pos_setpoint();
+	_navigator->set_can_loiter_at_sp(false);
+
+	switch (_dll_state) {
+	case DLL_STATE_FLYTOCOMMSHOLDWP: {
+		_mission_item.lat = (double)(_param_commsholdlat.get()) * 1.0e-7;
+		_mission_item.lon = (double)(_param_commsholdlon.get()) * 1.0e-7;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _param_commsholdalt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = _param_commsholdwaittime.get() < 0.0f ? 0.0f : _param_commsholdwaittime.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+		break;
+	}
+	case DLL_STATE_FLYTOAIRFIELDHOMEWP: {
+		_mission_item.lat = (double)(_param_airfieldhomelat.get()) * 1.0e-7;
+		_mission_item.lon = (double)(_param_airfieldhomelon.get()) * 1.0e-7;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.altitude = _param_airfieldhomealt.get();
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+		_mission_item.time_inside = _param_airfieldhomewaittime.get() < 0.0f ? 0.0f : _param_airfieldhomewaittime.get();
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+		break;
+	}
+	case DLL_STATE_TERMINATE: {
+		/* Request flight termination from the commander */
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
+		warnx("not switched to manual: request flight termination");
+		pos_sp_triplet->previous.valid = false;
+		pos_sp_triplet->current.valid = false;
+		pos_sp_triplet->next.valid = false;
+		break;
+	}
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+
+	/* convert mission item to current position setpoint and make it valid */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+DataLinkLoss::advance_dll()
+{
+	switch (_dll_state) {
+	case DLL_STATE_NONE:
+		/* Check the number of data link losses. If above home fly home directly */
+		/* if number of data link losses limit is not reached fly to comms hold wp */
+		if (_navigator->get_vstatus()->data_link_lost_counter > _param_numberdatalinklosses.get()) {
+			warnx("%d data link losses, limit is %d, fly to airfield home",
+					_navigator->get_vstatus()->data_link_lost_counter, _param_numberdatalinklosses.get());
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: too many DL losses, fly to airfield home");
+			_navigator->get_mission_result()->stay_in_failsafe = true;
+			_navigator->publish_mission_result();
+			_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+		} else {
+			if (!_param_skipcommshold.get()) {
+				warnx("fly to comms hold, datalink loss counter: %d", _navigator->get_vstatus()->data_link_lost_counter);
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to comms hold");
+				_dll_state = DLL_STATE_FLYTOCOMMSHOLDWP;
+			} else {
+				/* comms hold wp not active, fly to airfield home directly */
+				warnx("Skipping comms hold wp. Flying directly to airfield home");
+				mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home, comms hold skipped");
+				_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+			}
+		}
+		break;
+	case DLL_STATE_FLYTOCOMMSHOLDWP:
+		warnx("fly to airfield home");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: fly to airfield home");
+		_dll_state = DLL_STATE_FLYTOAIRFIELDHOMEWP;
+		_navigator->get_mission_result()->stay_in_failsafe = true;
+		_navigator->publish_mission_result();
+		break;
+	case DLL_STATE_FLYTOAIRFIELDHOMEWP:
+		_dll_state = DLL_STATE_TERMINATE;
+		warnx("time is up, state should have been changed manually by now");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no manual control, terminating");
+		_navigator->get_mission_result()->stay_in_failsafe = true;
+		_navigator->publish_mission_result();
+		break;
+	case DLL_STATE_TERMINATE:
+		warnx("dll end");
+		_dll_state = DLL_STATE_END;
+		break;
+
+	default:
+		break;
+	}
+}

src/modules/navigator/datalinkloss.h

@@ -0,0 +1,98 @@
+/***************************************************************************
+ *
+ *   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 datalinkloss.h
+ * Helper class for Data Link Loss Mode acording to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_DATALINKLOSS_H
+#define NAVIGATOR_DATALINKLOSS_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/Subscription.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class DataLinkLoss : public MissionBlock
+{
+public:
+	DataLinkLoss(Navigator *navigator, const char *name);
+
+	~DataLinkLoss();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	/* Params */
+	control::BlockParamFloat _param_commsholdwaittime;
+	control::BlockParamInt _param_commsholdlat; // * 1e7
+	control::BlockParamInt _param_commsholdlon; // * 1e7
+	control::BlockParamFloat _param_commsholdalt;
+	control::BlockParamInt _param_airfieldhomelat; // * 1e7
+	control::BlockParamInt _param_airfieldhomelon; // * 1e7
+	control::BlockParamFloat _param_airfieldhomealt;
+	control::BlockParamFloat _param_airfieldhomewaittime;
+	control::BlockParamInt _param_numberdatalinklosses;
+	control::BlockParamInt _param_skipcommshold;
+
+	enum DLLState {
+		DLL_STATE_NONE = 0,
+		DLL_STATE_FLYTOCOMMSHOLDWP = 1,
+		DLL_STATE_FLYTOAIRFIELDHOMEWP = 2,
+		DLL_STATE_TERMINATE = 3,
+		DLL_STATE_END = 4
+	} _dll_state;
+
+	/**
+	 * Set the DLL item
+	 */
+	void		set_dll_item();
+
+	/**
+	 * Move to next DLL item
+	 */
+	void		advance_dll();
+
+};
+#endif

src/modules/navigator/datalinkloss_params.c

@@ -0,0 +1,126 @@
+/****************************************************************************
+ *
+ *   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 datalinkloss_params.c
+ *
+ * Parameters for DLL
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * Data Link Loss parameters, accessible via MAVLink
+ */
+
+/**
+ * Comms hold wait time
+ *
+ * The amount of time in seconds the system should wait at the comms hold waypoint
+ *
+ * @unit seconds
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_CH_T, 120.0f);
+
+/**
+ * Comms hold Lat
+ *
+ * Latitude of comms hold waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CH_LAT, -266072120);
+
+/**
+ * Comms hold Lon
+ *
+ * Longitude of comms hold waypoint
+ *
+ * @unit degrees * 1e7
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CH_LON, 1518453890);
+
+/**
+ * Comms hold alt
+ *
+ * Altitude of comms hold waypoint
+ *
+ * @unit m
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_CH_ALT, 600.0f);
+
+/**
+ * Aifield hole wait time
+ *
+ * The amount of time in seconds the system should wait at the airfield home waypoint
+ *
+ * @unit seconds
+ * @min 0.0
+ * @group DLL
+ */
+PARAM_DEFINE_FLOAT(NAV_DLL_AH_T, 120.0f);
+
+/**
+ * Number of allowed Datalink timeouts
+ *
+ * After more than this number of data link timeouts the aircraft returns home directly
+ *
+ * @group DLL
+ * @min 0
+ * @max 1000
+ */
+PARAM_DEFINE_INT32(NAV_DLL_N, 2);
+
+/**
+ * Skip comms hold wp
+ *
+ * If set to 1 the system will skip the comms hold wp on data link loss and will directly fly to
+ * airfield home
+ *
+ * @group DLL
+ * @min 0
+ * @max 1
+ */
+PARAM_DEFINE_INT32(NAV_DLL_CHSK, 0);

src/modules/navigator/enginefailure.cpp

@@ -0,0 +1,149 @@
+/****************************************************************************
+ *
+ *   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 enginefailure.cpp
+ * Helper class for a fixedwing engine failure mode
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+
+#include "navigator.h"
+#include "enginefailure.h"
+
+#define DELAY_SIGMA	0.01f
+
+EngineFailure::EngineFailure(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_ef_state(EF_STATE_NONE)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+EngineFailure::~EngineFailure()
+{
+}
+
+void
+EngineFailure::on_inactive()
+{
+	_ef_state = EF_STATE_NONE;
+}
+
+void
+EngineFailure::on_activation()
+{
+	_ef_state = EF_STATE_NONE;
+	advance_ef();
+	set_ef_item();
+}
+
+void
+EngineFailure::on_active()
+{
+	if (is_mission_item_reached()) {
+		advance_ef();
+		set_ef_item();
+	}
+}
+
+void
+EngineFailure::set_ef_item()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* make sure we have the latest params */
+	updateParams();
+
+	set_previous_pos_setpoint();
+	_navigator->set_can_loiter_at_sp(false);
+
+	switch (_ef_state) {
+	case EF_STATE_LOITERDOWN: {
+		//XXX create mission item at ground (below?) here
+
+		_mission_item.lat = _navigator->get_global_position()->lat;
+		_mission_item.lon = _navigator->get_global_position()->lon;
+		_mission_item.altitude_is_relative = false;
+		 //XXX setting altitude to a very low value, evaluate other options
+		_mission_item.altitude = _navigator->get_home_position()->alt - 1000.0f;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+		break;
+	}
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+
+	/* convert mission item to current position setpoint and make it valid */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+EngineFailure::advance_ef()
+{
+	switch (_ef_state) {
+	case EF_STATE_NONE:
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: Engine failure. Loitering down");
+		_ef_state = EF_STATE_LOITERDOWN;
+		break;
+	default:
+		break;
+	}
+}

src/modules/navigator/enginefailure.h

@@ -0,0 +1,83 @@
+/***************************************************************************
+ *
+ *   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 enginefailure.h
+ * Helper class for a fixedwing engine failure mode
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_ENGINEFAILURE_H
+#define NAVIGATOR_ENGINEFAILURE_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/Subscription.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class EngineFailure : public MissionBlock
+{
+public:
+	EngineFailure(Navigator *navigator, const char *name);
+
+	~EngineFailure();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	enum EFState {
+		EF_STATE_NONE = 0,
+		EF_STATE_LOITERDOWN = 1,
+	} _ef_state;
+
+	/**
+	 * Set the DLL item
+	 */
+	void		set_ef_item();
+
+	/**
+	 * Move to next EF item
+	 */
+	void		advance_ef();
+
+};
+#endif

src/modules/navigator/gpsfailure.cpp

@@ -0,0 +1,178 @@
+/****************************************************************************
+ *
+ *   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 gpsfailure.cpp
+ * Helper class for gpsfailure mode according to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "navigator.h"
+#include "gpsfailure.h"
+
+#define DELAY_SIGMA	0.01f
+
+GpsFailure::GpsFailure(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_param_loitertime(this, "LT"),
+	_param_openlooploiter_roll(this, "R"),
+	_param_openlooploiter_pitch(this, "P"),
+	_param_openlooploiter_thrust(this, "TR"),
+	_gpsf_state(GPSF_STATE_NONE),
+	_timestamp_activation(0)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+GpsFailure::~GpsFailure()
+{
+}
+
+void
+GpsFailure::on_inactive()
+{
+	/* reset GPSF state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+		_gpsf_state = GPSF_STATE_NONE;
+	}
+}
+
+void
+GpsFailure::on_activation()
+{
+	_gpsf_state = GPSF_STATE_NONE;
+	_timestamp_activation = hrt_absolute_time();
+	updateParams();
+	advance_gpsf();
+	set_gpsf_item();
+}
+
+void
+GpsFailure::on_active()
+{
+
+	switch (_gpsf_state) {
+	case GPSF_STATE_LOITER: {
+		/* Position controller does not run in this mode:
+		 * navigator has to publish an attitude setpoint */
+		_navigator->get_att_sp()->roll_body = M_DEG_TO_RAD_F * _param_openlooploiter_roll.get();
+		_navigator->get_att_sp()->pitch_body = M_DEG_TO_RAD_F * _param_openlooploiter_pitch.get();
+		_navigator->get_att_sp()->thrust = _param_openlooploiter_thrust.get();
+		_navigator->publish_att_sp();
+
+		/* Measure time */
+		hrt_abstime elapsed = hrt_elapsed_time(&_timestamp_activation);
+
+		//warnx("open loop loiter, posctl enabled %u, elapsed %.1fs, thrust %.2f",
+				//_navigator->get_control_mode()->flag_control_position_enabled, elapsed * 1e-6, (double)_param_openlooploiter_thrust.get());
+		if (elapsed > _param_loitertime.get() * 1e6f) {
+			/* no recovery, adavance the state machine */
+			warnx("gps not recovered, switch to next state");
+			advance_gpsf();
+		}
+		break;
+	}
+	case GPSF_STATE_TERMINATE:
+		set_gpsf_item();
+		advance_gpsf();
+		break;
+	default:
+		break;
+	}
+}
+
+void
+GpsFailure::set_gpsf_item()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	/* Set pos sp triplet to invalid to stop pos controller */
+	pos_sp_triplet->previous.valid = false;
+	pos_sp_triplet->current.valid = false;
+	pos_sp_triplet->next.valid = false;
+
+	switch (_gpsf_state) {
+	case GPSF_STATE_TERMINATE: {
+		/* Request flight termination from the commander */
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
+		warnx("gps fail: request flight termination");
+	}
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+GpsFailure::advance_gpsf()
+{
+	updateParams();
+
+	switch (_gpsf_state) {
+	case GPSF_STATE_NONE:
+		_gpsf_state = GPSF_STATE_LOITER;
+		warnx("gpsf loiter");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: open loop loiter");
+		break;
+	case GPSF_STATE_LOITER:
+		_gpsf_state = GPSF_STATE_TERMINATE;
+		warnx("gpsf terminate");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: no gps recovery, termination");
+		warnx("mavlink sent");
+		break;
+	case GPSF_STATE_TERMINATE:
+		warnx("gpsf end");
+		_gpsf_state = GPSF_STATE_END;
+	default:
+		break;
+	}
+}

src/modules/navigator/gpsfailure.h

@@ -0,0 +1,97 @@
+/***************************************************************************
+ *
+ *   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 gpsfailure.h
+ * Helper class for Data Link Loss Mode according to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_GPSFAILURE_H
+#define NAVIGATOR_GPSFAILURE_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/uORB.h>
+#include <uORB/Publication.hpp>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+
+#include <drivers/drv_hrt.h>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class GpsFailure : public MissionBlock
+{
+public:
+	GpsFailure(Navigator *navigator, const char *name);
+
+	~GpsFailure();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	/* Params */
+	control::BlockParamFloat _param_loitertime;
+	control::BlockParamFloat _param_openlooploiter_roll;
+	control::BlockParamFloat _param_openlooploiter_pitch;
+	control::BlockParamFloat _param_openlooploiter_thrust;
+
+	enum GPSFState {
+		GPSF_STATE_NONE = 0,
+		GPSF_STATE_LOITER = 1,
+		GPSF_STATE_TERMINATE = 2,
+		GPSF_STATE_END = 3,
+	} _gpsf_state;
+
+	hrt_abstime _timestamp_activation; //*< timestamp when this mode was activated */
+
+	/**
+	 * Set the GPSF item
+	 */
+	void		set_gpsf_item();
+
+	/**
+	 * Move to next GPSF item
+	 */
+	void		advance_gpsf();
+
+};
+#endif

src/modules/navigator/gpsfailure_params.c

@@ -0,0 +1,97 @@
+/****************************************************************************
+ *
+ *   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 gpsfailure_params.c
+ *
+ * Parameters for GPSF navigation mode
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * GPS Failure Navigation Mode parameters, accessible via MAVLink
+ */
+
+/**
+ * Loiter time
+ *
+ * The amount of time in seconds the system should do open loop loiter and wait for gps recovery
+ * before it goes into flight termination.
+ *
+ * @unit seconds
+ * @min 0.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_LT, 30.0f);
+
+/**
+ * Open loop loiter roll
+ *
+ * Roll in degrees during the open loop loiter
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 30.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_R, 15.0f);
+
+/**
+ * Open loop loiter pitch
+ *
+ * Pitch in degrees during the open loop loiter
+ *
+ * @unit deg
+ * @min -30.0
+ * @max 30.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_P, 0.0f);
+
+/**
+ * Open loop loiter thrust
+ *
+ * Thrust value which is set during the open loop loiter
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group GPSF
+ */
+PARAM_DEFINE_FLOAT(NAV_GPSF_TR, 0.7f);
+
+

src/modules/navigator/rcloss.cpp

@@ -0,0 +1,182 @@
+/****************************************************************************
+ *
+ *   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 rcloss.cpp
+ * Helper class for RC Loss Mode according to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <string.h>
+#include <stdlib.h>
+#include <math.h>
+#include <fcntl.h>
+
+#include <mavlink/mavlink_log.h>
+#include <systemlib/err.h>
+#include <geo/geo.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/mission.h>
+#include <uORB/topics/home_position.h>
+
+#include "navigator.h"
+#include "datalinkloss.h"
+
+#define DELAY_SIGMA	0.01f
+
+RCLoss::RCLoss(Navigator *navigator, const char *name) :
+	MissionBlock(navigator, name),
+	_param_loitertime(this, "LT"),
+	_rcl_state(RCL_STATE_NONE)
+{
+	/* load initial params */
+	updateParams();
+	/* initial reset */
+	on_inactive();
+}
+
+RCLoss::~RCLoss()
+{
+}
+
+void
+RCLoss::on_inactive()
+{
+	/* reset RCL state only if setpoint moved */
+	if (!_navigator->get_can_loiter_at_sp()) {
+		_rcl_state = RCL_STATE_NONE;
+	}
+}
+
+void
+RCLoss::on_activation()
+{
+	_rcl_state = RCL_STATE_NONE;
+	updateParams();
+	advance_rcl();
+	set_rcl_item();
+}
+
+void
+RCLoss::on_active()
+{
+	if (is_mission_item_reached()) {
+		updateParams();
+		advance_rcl();
+		set_rcl_item();
+	}
+}
+
+void
+RCLoss::set_rcl_item()
+{
+	struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet();
+
+	set_previous_pos_setpoint();
+	_navigator->set_can_loiter_at_sp(false);
+
+	switch (_rcl_state) {
+	case RCL_STATE_LOITER: {
+		_mission_item.lat = _navigator->get_global_position()->lat;
+		_mission_item.lon = _navigator->get_global_position()->lon;
+		_mission_item.altitude = _navigator->get_global_position()->alt;
+		_mission_item.altitude_is_relative = false;
+		_mission_item.yaw = NAN;
+		_mission_item.loiter_radius = _navigator->get_loiter_radius();
+		_mission_item.loiter_direction = 1;
+		_mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+		_mission_item.acceptance_radius = _navigator->get_acceptance_radius();
+		_mission_item.time_inside = _param_loitertime.get() < 0.0f ? 0.0f : _param_loitertime.get();
+		_mission_item.pitch_min = 0.0f;
+		_mission_item.autocontinue = true;
+		_mission_item.origin = ORIGIN_ONBOARD;
+
+		_navigator->set_can_loiter_at_sp(true);
+		break;
+	}
+	case RCL_STATE_TERMINATE: {
+		/* Request flight termination from the commander */
+		_navigator->get_mission_result()->flight_termination = true;
+		_navigator->publish_mission_result();
+		warnx("rc not recovered: request flight termination");
+		pos_sp_triplet->previous.valid = false;
+		pos_sp_triplet->current.valid = false;
+		pos_sp_triplet->next.valid = false;
+		break;
+	}
+	default:
+		break;
+	}
+
+	reset_mission_item_reached();
+
+	/* convert mission item to current position setpoint and make it valid */
+	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);
+	pos_sp_triplet->next.valid = false;
+
+	_navigator->set_position_setpoint_triplet_updated();
+}
+
+void
+RCLoss::advance_rcl()
+{
+	switch (_rcl_state) {
+	case RCL_STATE_NONE:
+		if (_param_loitertime.get() > 0.0f) {
+			warnx("RC loss, OBC mode, loiter");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, loitering");
+			_rcl_state = RCL_STATE_LOITER;
+		} else {
+			warnx("RC loss, OBC mode, slip loiter, terminate");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: rc loss, terminating");
+			_rcl_state = RCL_STATE_TERMINATE;
+			_navigator->get_mission_result()->stay_in_failsafe = true;
+			_navigator->publish_mission_result();
+		}
+		break;
+	case RCL_STATE_LOITER:
+		_rcl_state = RCL_STATE_TERMINATE;
+		warnx("time is up, no RC regain, terminating");
+		mavlink_log_info(_navigator->get_mavlink_fd(), "#audio: RC not regained, terminating");
+		_navigator->get_mission_result()->stay_in_failsafe = true;
+		_navigator->publish_mission_result();
+		break;
+	case RCL_STATE_TERMINATE:
+		warnx("rcl end");
+		_rcl_state = RCL_STATE_END;
+		break;
+	default:
+		break;
+	}
+}

src/modules/navigator/rcloss.h

@@ -0,0 +1,88 @@
+/***************************************************************************
+ *
+ *   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 rcloss.h
+ * Helper class for RC Loss Mode acording to the OBC rules
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#ifndef NAVIGATOR_RCLOSS_H
+#define NAVIGATOR_RCLOSS_H
+
+#include <controllib/blocks.hpp>
+#include <controllib/block/BlockParam.hpp>
+
+#include <uORB/Subscription.hpp>
+
+#include "navigator_mode.h"
+#include "mission_block.h"
+
+class Navigator;
+
+class RCLoss : public MissionBlock
+{
+public:
+	RCLoss(Navigator *navigator, const char *name);
+
+	~RCLoss();
+
+	virtual void on_inactive();
+
+	virtual void on_activation();
+
+	virtual void on_active();
+
+private:
+	/* Params */
+	control::BlockParamFloat _param_loitertime;
+
+	enum RCLState {
+		RCL_STATE_NONE = 0,
+		RCL_STATE_LOITER = 1,
+		RCL_STATE_TERMINATE = 2,
+		RCL_STATE_END = 3
+	} _rcl_state;
+
+	/**
+	 * Set the RCL item
+	 */
+	void		set_rcl_item();
+
+	/**
+	 * Move to next RCL item
+	 */
+	void		advance_rcl();
+
+};
+#endif

src/modules/navigator/rcloss_params.c

@@ -0,0 +1,60 @@
+/****************************************************************************
+ *
+ *   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 rcloss_params.c
+ *
+ * Parameters for RC Loss (OBC)
+ *
+ * @author Thomas Gubler <thomasgubler@gmail.com>
+ */
+
+#include <nuttx/config.h>
+
+#include <systemlib/param/param.h>
+
+/*
+ * OBC RC Loss mode parameters, accessible via MAVLink
+ */
+
+/**
+ * Loiter Time
+ *
+ * The amount of time in seconds the system should loiter at current position before termination
+ * Set to -1 to make the system skip loitering
+ *
+ * @unit seconds
+ * @min -1.0
+ * @group RCL
+ */
+PARAM_DEFINE_FLOAT(NAV_RCL_LT, 120.0f);

src/modules/uORB/topics/multirotor_motor_limits.h

@@ -0,0 +1,69 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file multirotor_motor_limits.h
+ *
+ * Definition of multirotor_motor_limits  topic
+ */
+
+#ifndef MULTIROTOR_MOTOR_LIMITS_H_
+#define MULTIROTOR_MOTOR_LIMITS_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Motor limits
+ */
+struct multirotor_motor_limits_s {
+        uint8_t roll_pitch	: 1; // roll/pitch limit reached
+        uint8_t yaw		: 1; // yaw limit reached
+        uint8_t throttle_lower	: 1; // lower throttle limit reached
+        uint8_t throttle_upper	: 1; // upper throttle limit reached
+        uint8_t reserved	: 4;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(multirotor_motor_limits);
+
+#endif

src/modules/uORB/topics/vtol_vehicle_status.h

@@ -0,0 +1,66 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file vtol_status.h
+ *
+ * Vtol status topic
+ *
+ */
+
+#ifndef TOPIC_VTOL_STATUS_H
+#define TOPIC_VTOL_STATUS_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/* Indicates in which mode the vtol aircraft is in */
+struct vtol_vehicle_status_s {
+
+	uint64_t	timestamp;	/**< Microseconds since system boot */
+	bool vtol_in_rw_mode;	/*true: vtol vehicle is in rotating wing mode */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(vtol_vehicle_status);
+
+#endif

src/modules/vtol_att_control/module.mk

@@ -0,0 +1,40 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# VTOL attitude controller
+#
+
+MODULE_COMMAND	= vtol_att_control
+
+SRCS		= vtol_att_control_main.cpp

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -0,0 +1,642 @@
+/****************************************************************************
+ *
+ *   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 VTOL_att_control_main.cpp
+ * Implementation of an attitude controller for VTOL airframes. This module receives data
+ * from both the fixed wing- and the multicopter attitude controllers and processes it.
+ * It computes the correct actuator controls depending on which mode the vehicle is in (hover,forward-
+ * flight or transition). It also publishes the resulting controls on the actuator controls topics.
+ *
+ * @author Roman Bapst 		<bapstr@ethz.ch>
+ * @author Lorenz Meier 	<lm@inf.ethz.ch>
+ * @author Thomas Gubler	<thomasgubler@gmail.com>
+ *
+ */
+
+#include <nuttx/config.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <drivers/drv_hrt.h>
+#include <arch/board/board.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/vehicle_rates_setpoint.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vtol_vehicle_status.h>
+#include <uORB/topics/actuator_armed.h>
+#include <uORB/topics/parameter_update.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/circuit_breaker.h>
+#include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+
+#include "drivers/drv_pwm_output.h"
+#include <nuttx/fs/nxffs.h>
+#include <nuttx/fs/ioctl.h>
+
+#include <nuttx/mtd.h>
+
+#include <fcntl.h>
+
+
+extern "C" __EXPORT int vtol_att_control_main(int argc, char *argv[]);
+
+class VtolAttitudeControl
+{
+public:
+
+	VtolAttitudeControl();
+	~VtolAttitudeControl();
+
+	int start();	/* start the task and return OK on success */
+
+
+private:
+//******************flags & handlers******************************************************
+	bool _task_should_exit;
+	int _control_task;		//task handle for VTOL attitude controller
+
+	/* handlers for subscriptions */
+	int		_v_att_sub;				//vehicle attitude subscription
+	int		_v_att_sp_sub;			//vehicle attitude setpoint subscription
+	int		_v_rates_sp_sub;		//vehicle rates setpoint subscription
+	int		_v_control_mode_sub;	//vehicle control mode subscription
+	int		_params_sub;			//parameter updates subscription
+	int		_manual_control_sp_sub;	//manual control setpoint subscription
+	int		_armed_sub;				//arming status subscription
+
+	int 	_actuator_inputs_mc;	//topic on which the mc_att_controller publishes actuator inputs
+	int 	_actuator_inputs_fw;	//topic on which the fw_att_controller publishes actuator inputs
+
+	//handlers for publishers
+	orb_advert_t	_actuators_0_pub;		//input for the mixer (roll,pitch,yaw,thrust)
+	orb_advert_t 	_actuators_1_pub;
+	orb_advert_t	_vtol_vehicle_status_pub;
+//*******************data containers***********************************************************
+	struct vehicle_attitude_s			_v_att;				//vehicle attitude
+	struct vehicle_attitude_setpoint_s	_v_att_sp;			//vehicle attitude setpoint
+	struct vehicle_rates_setpoint_s		_v_rates_sp;		//vehicle rates setpoint
+	struct manual_control_setpoint_s	_manual_control_sp; //manual control setpoint
+	struct vehicle_control_mode_s		_v_control_mode;	//vehicle control mode
+	struct vtol_vehicle_status_s 		_vtol_vehicle_status;
+	struct actuator_controls_s			_actuators_out_0;	//actuator controls going to the mc mixer
+	struct actuator_controls_s			_actuators_out_1;	//actuator controls going to the fw mixer (used for elevons)
+	struct actuator_controls_s			_actuators_mc_in;	//actuator controls from mc_att_control
+	struct actuator_controls_s			_actuators_fw_in;	//actuator controls from fw_att_control
+	struct actuator_armed_s				_armed;				//actuator arming status
+
+	struct {
+		float min_pwm_mc;	//pwm value for idle in mc mode
+	} _params;
+
+	struct {
+		param_t min_pwm_mc;
+	} _params_handles;
+
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
+
+	/* for multicopters it is usual to have a non-zero idle speed of the engines
+	 * for fixed wings we want to have an idle speed of zero since we do not want
+	 * to waste energy when gliding. */
+	bool flag_idle_mc;		//false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"
+
+//*****************Member functions***********************************************************************
+
+	void 		task_main();	//main task
+	static void	task_main_trampoline(int argc, char *argv[]);	//Shim for calling task_main from task_create.
+
+	void		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
+	void		vehicle_manual_poll();			//Check for changes in manual inputs.
+	void		arming_status_poll();			//Check for arming status updates.
+	void 		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output
+	void 		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
+	void 		parameters_update_poll();		//Check if parameters have changed
+	int 		parameters_update();			//Update local paraemter cache
+	void  		fill_mc_att_control_output();	//write mc_att_control results to actuator message
+	void		fill_fw_att_control_output();	//write fw_att_control results to actuator message
+	void 		set_idle_fw();
+	void 		set_idle_mc();
+};
+
+namespace VTOL_att_control
+{
+VtolAttitudeControl *g_control;
+}
+
+/**
+* Constructor
+*/
+VtolAttitudeControl::VtolAttitudeControl() :
+	_task_should_exit(false),
+	_control_task(-1),
+
+	//init subscription handlers
+	_v_att_sub(-1),
+	_v_att_sp_sub(-1),
+	_v_control_mode_sub(-1),
+	_params_sub(-1),
+	_manual_control_sp_sub(-1),
+	_armed_sub(-1),
+
+	//init publication handlers
+	_actuators_0_pub(-1),
+	_actuators_1_pub(-1),
+	_vtol_vehicle_status_pub(-1),
+
+	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
+{
+
+	flag_idle_mc = true;	/*assume we always start in mc mode for a VTOL airframe */
+
+	memset(& _vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
+	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/
+	memset(&_v_att, 0, sizeof(_v_att));
+	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
+	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_vtol_vehicle_status, 0, sizeof(_vtol_vehicle_status));
+	memset(&_actuators_out_0, 0, sizeof(_actuators_out_0));
+	memset(&_actuators_out_1, 0, sizeof(_actuators_out_1));
+	memset(&_actuators_mc_in, 0, sizeof(_actuators_mc_in));
+	memset(&_actuators_fw_in, 0, sizeof(_actuators_fw_in));
+	memset(&_armed, 0, sizeof(_armed));
+
+	_params_handles.min_pwm_mc = param_find("PWM_MIN");
+
+	/* fetch initial parameter values */
+	parameters_update();
+}
+
+/**
+* Destructor
+*/
+VtolAttitudeControl::~VtolAttitudeControl()
+{
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	VTOL_att_control::g_control = nullptr;
+}
+
+/**
+* Check for changes in vehicle control mode.
+*/
+void VtolAttitudeControl::vehicle_control_mode_poll()
+{
+	bool updated;
+
+	/* Check if vehicle control mode has changed */
+	orb_check(_v_control_mode_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_control_mode), _v_control_mode_sub, &_v_control_mode);
+	}
+}
+
+/**
+* Check for changes in manual inputs.
+*/
+void VtolAttitudeControl::vehicle_manual_poll()
+{
+	bool updated;
+
+	/* get pilots inputs */
+	orb_check(_manual_control_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sp_sub, &_manual_control_sp);
+	}
+}
+/**
+* Check for arming status updates.
+*/
+void VtolAttitudeControl::arming_status_poll()
+{
+	/* check if there is a new setpoint */
+	bool updated;
+	orb_check(_armed_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+	}
+}
+
+/**
+* Check for inputs from mc attitude controller.
+*/
+void VtolAttitudeControl::actuator_controls_mc_poll()
+{
+	bool updated;
+	orb_check(_actuator_inputs_mc, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc , &_actuators_mc_in);
+	}
+}
+
+/**
+* Check for inputs from fw attitude controller.
+*/
+void VtolAttitudeControl::actuator_controls_fw_poll()
+{
+	bool updated;
+	orb_check(_actuator_inputs_fw, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw , &_actuators_fw_in);
+	}
+}
+
+/**
+* Check for parameter updates.
+*/
+void
+VtolAttitudeControl::parameters_update_poll()
+{
+	bool updated;
+
+	/* Check if parameters have changed */
+	orb_check(_params_sub, &updated);
+
+	if (updated) {
+		struct parameter_update_s param_update;
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+		parameters_update();
+	}
+}
+
+/**
+* Update parameters.
+*/
+int
+VtolAttitudeControl::parameters_update()
+{
+	/* idle pwm */
+	float v;
+	param_get(_params_handles.min_pwm_mc, &v);
+	_params.min_pwm_mc = v;
+
+	return OK;
+}
+
+/**
+* Prepare message to acutators with data from mc attitude controller.
+*/
+void VtolAttitudeControl::fill_mc_att_control_output()
+{
+	_actuators_out_0.control[0] = _actuators_mc_in.control[0];
+	_actuators_out_0.control[1] = _actuators_mc_in.control[1];
+	_actuators_out_0.control[2] = _actuators_mc_in.control[2];
+	_actuators_out_0.control[3] = _actuators_mc_in.control[3];
+	//set neutral position for elevons
+	_actuators_out_1.control[0] = 0;	//roll elevon
+	_actuators_out_1.control[1] = 0;	//pitch elevon
+}
+
+/**
+* Prepare message to acutators with data from fw attitude controller.
+*/
+void VtolAttitudeControl::fill_fw_att_control_output()
+{
+	/*For the first test in fw mode, only use engines for thrust!!!*/
+	_actuators_out_0.control[0] = 0;
+	_actuators_out_0.control[1] = 0;
+	_actuators_out_0.control[2] = 0;
+	_actuators_out_0.control[3] = _actuators_fw_in.control[3];
+	/*controls for the elevons */
+	_actuators_out_1.control[0] = _actuators_fw_in.control[0];	/*roll elevon*/
+	_actuators_out_1.control[1] = _actuators_fw_in.control[1];	/*pitch elevon */
+}
+
+/**
+* Adjust idle speed for fw mode.
+*/
+void VtolAttitudeControl::set_idle_fw()
+{
+	int ret;
+	char *dev = PWM_OUTPUT_DEVICE_PATH;
+	int fd = open(dev, 0);
+
+	if (fd < 0) {err(1, "can't open %s", dev);}
+
+	unsigned pwm_value = PWM_LOWEST_MIN;
+	struct pwm_output_values pwm_values;
+	memset(&pwm_values, 0, sizeof(pwm_values));
+
+	for (unsigned i = 0; i < 4; i++) {
+
+		pwm_values.values[i] = pwm_value;
+		pwm_values.channel_count++;
+	}
+
+	ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+
+	if (ret != OK) {errx(ret, "failed setting min values");}
+
+	close(fd);
+}
+
+/**
+* Adjust idle speed for mc mode.
+*/
+void VtolAttitudeControl::set_idle_mc()
+{
+	int ret;
+	unsigned servo_count;
+	char *dev = PWM_OUTPUT_DEVICE_PATH;
+	int fd = open(dev, 0);
+
+	if (fd < 0) {err(1, "can't open %s", dev);}
+
+	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
+	unsigned pwm_value = 1100;
+	struct pwm_output_values pwm_values;
+	memset(&pwm_values, 0, sizeof(pwm_values));
+
+	for (unsigned i = 0; i < 4; i++) {
+		pwm_values.values[i] = pwm_value;
+		pwm_values.channel_count++;
+	}
+
+	ret = ioctl(fd, PWM_SERVO_SET_MIN_PWM, (long unsigned int)&pwm_values);
+
+	if (ret != OK) {errx(ret, "failed setting min values");}
+
+	close(fd);
+}
+
+void
+VtolAttitudeControl::task_main_trampoline(int argc, char *argv[])
+{
+	VTOL_att_control::g_control->task_main();
+}
+
+void VtolAttitudeControl::task_main()
+{
+	warnx("started");
+	fflush(stdout);
+
+	/* do subscriptions */
+	_v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_v_rates_sp_sub        = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
+	_v_att_sub             = orb_subscribe(ORB_ID(vehicle_attitude));
+	_v_control_mode_sub    = orb_subscribe(ORB_ID(vehicle_control_mode));
+	_params_sub            = orb_subscribe(ORB_ID(parameter_update));
+	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_armed_sub             = orb_subscribe(ORB_ID(actuator_armed));
+
+	_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
+	_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
+
+	parameters_update();  /*initialize parameter cache/*
+
+	/* wakeup source*/
+	struct pollfd fds[3];	/*input_mc, input_fw, parameters*/
+
+	fds[0].fd     = _actuator_inputs_mc;
+	fds[0].events = POLLIN;
+	fds[1].fd     = _actuator_inputs_fw;
+	fds[1].events = POLLIN;
+	fds[2].fd     = _params_sub;
+	fds[2].events = POLLIN;
+
+	while (!_task_should_exit) {
+		/*Advertise/Publish vtol vehicle status*/
+		if (_vtol_vehicle_status_pub > 0) {
+			orb_publish(ORB_ID(vtol_vehicle_status), _vtol_vehicle_status_pub, &_vtol_vehicle_status);
+
+		} else {
+			_vtol_vehicle_status.timestamp = hrt_absolute_time();
+			_vtol_vehicle_status_pub = orb_advertise(ORB_ID(vtol_vehicle_status), &_vtol_vehicle_status);
+		}
+
+		/* wait for up to 100ms for data */
+		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
+
+
+		/* timed out - periodic check for _task_should_exit */
+		if (pret == 0) {
+			continue;
+		}
+
+		/* this is undesirable but not much we can do - might want to flag unhappy status */
+		if (pret < 0) {
+			warn("poll error %d, %d", pret, errno);
+			/* sleep a bit before next try */
+			usleep(100000);
+			continue;
+		}
+
+		if (fds[2].revents & POLLIN) {	//parameters were updated, read them now
+			/* read from param to clear updated flag */
+			struct parameter_update_s update;
+			orb_copy(ORB_ID(parameter_update), _params_sub, &update);
+
+			/* update parameters from storage */
+			parameters_update();
+		}
+
+		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
+		vehicle_manual_poll();			//Check for changes in manual inputs.
+		arming_status_poll();			//Check for arming status updates.
+		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output
+		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
+		parameters_update_poll();
+
+		if (_manual_control_sp.aux1 <= 0.0f) {		/* vehicle is in mc mode */
+			_vtol_vehicle_status.vtol_in_rw_mode = true;
+
+			if (!flag_idle_mc) {	/* we want to adjust idle speed for mc mode */
+				set_idle_mc();
+				flag_idle_mc = true;
+			}
+
+			/* got data from mc_att_controller */
+			if (fds[0].revents & POLLIN) {
+				vehicle_manual_poll();	/* update remote input */
+				orb_copy(ORB_ID(actuator_controls_virtual_mc), _actuator_inputs_mc, &_actuators_mc_in);
+
+				fill_mc_att_control_output();
+
+				if (_actuators_0_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
+
+				} else {
+					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
+				}
+
+				if (_actuators_1_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
+
+				} else {
+					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+				}
+			}
+		}
+
+		if (_manual_control_sp.aux1 >= 0.0f) {			/* vehicle is in fw mode */
+			_vtol_vehicle_status.vtol_in_rw_mode = false;
+
+			if (flag_idle_mc) {	/* we want to adjust idle speed for fixed wing mode */
+				set_idle_fw();
+				flag_idle_mc = false;
+			}
+
+			if (fds[1].revents & POLLIN) {		/* got data from fw_att_controller */
+				orb_copy(ORB_ID(actuator_controls_virtual_fw), _actuator_inputs_fw, &_actuators_fw_in);
+				vehicle_manual_poll();	//update remote input
+
+				fill_fw_att_control_output();
+
+				if (_actuators_0_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);
+
+				} else {
+					_actuators_0_pub = orb_advertise(ORB_ID(actuator_controls_0), &_actuators_out_0);
+				}
+
+				if (_actuators_1_pub > 0) {
+					orb_publish(ORB_ID(actuator_controls_1), _actuators_1_pub, &_actuators_out_1);
+
+				} else {
+					_actuators_1_pub = orb_advertise(ORB_ID(actuator_controls_1), &_actuators_out_1);
+				}
+			}
+		}
+	}
+
+	warnx("exit");
+	_control_task = -1;
+	_exit(0);
+}
+
+int
+VtolAttitudeControl::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("vtol_att_control",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 10,
+				       2048,
+				       (main_t)&VtolAttitudeControl::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+
+int vtol_att_control_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "usage: vtol_att_control {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (VTOL_att_control::g_control != nullptr) {
+			errx(1, "already running");
+		}
+
+		VTOL_att_control::g_control = new VtolAttitudeControl;
+
+		if (VTOL_att_control::g_control == nullptr) {
+			errx(1, "alloc failed");
+		}
+
+		if (OK != VTOL_att_control::g_control->start()) {
+			delete VTOL_att_control::g_control;
+			VTOL_att_control::g_control = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (VTOL_att_control::g_control == nullptr) {
+			errx(1, "not running");
+		}
+
+		delete VTOL_att_control::g_control;
+		VTOL_att_control::g_control = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (VTOL_att_control::g_control) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}