Merge branch 'master' of https://github.com/PX4/Firmware into fmuv2_bringup

32 files changed, 1347 insertions, 705 deletions

diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index ecd31a073..be8968a4e 100644
--- a/src/drivers/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -109,7 +109,7 @@ int ar_multiplexing_init()
 {
 	int		fd;
 	
-	fd = open(GPIO_DEVICE_PATH, 0);
+	fd = open(PX4FMU_DEVICE_PATH, 0);
 
 	if (fd < 0) {
 		warn("GPIO: open fail");
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 5b06f91a8..c7c25048a 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -63,7 +63,8 @@
  * they also export GPIO-like things.  This is always the GPIOs on the
  * main board.
  */
-# define GPIO_DEVICE_PATH	"/dev/px4fmu"
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+# define PX4IO_DEVICE_PATH	"/dev/px4io"
 
 #endif
 
@@ -92,22 +93,17 @@
  * they also export GPIO-like things.  This is always the GPIOs on the
  * main board.
  */
-# define GPIO_DEVICE_PATH	"/dev/px4fmu"
+# define PX4FMU_DEVICE_PATH	"/dev/px4fmu"
+# define PX4IO_DEVICE_PATH	"/dev/px4io"
 
 #endif
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 /* no GPIO driver on the PX4IOv1 board */
-# define GPIO_DEVICE_PATH	"/nonexistent"
 #endif
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V2
 /* no GPIO driver on the PX4IOv2 board */
-# define GPIO_DEVICE_PATH	"/nonexistent"
-#endif
-
-#ifndef GPIO_DEVICE_PATH
-#  error No GPIO support for this board.
 #endif
 
 /*
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 56af71059..52a667403 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -115,6 +115,15 @@ ORB_DECLARE(output_pwm);
 /** clear the 'ARM ok' bit, which deactivates the safety switch */
 #define PWM_SERVO_CLEAR_ARM_OK	_IOC(_PWM_SERVO_BASE, 6)
 
+/** start DSM bind */
+#define DSM_BIND_START	_IOC(_PWM_SERVO_BASE, 7)
+
+/** stop DSM bind */
+#define DSM_BIND_STOP	_IOC(_PWM_SERVO_BASE, 8)
+
+/** Power up DSM receiver */
+#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9)
+
 /** set a single servo to a specific value */
 #define PWM_SERVO_SET(_servo)	_IOC(_PWM_SERVO_BASE, 0x20 + _servo)
 
diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp
index 762c257aa..b579db715 100644
--- a/src/drivers/gps/ubx.cpp
+++ b/src/drivers/gps/ubx.cpp
@@ -3,6 +3,7 @@
  *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -55,13 +56,16 @@
 
 #include "ubx.h"
 
-#define UBX_CONFIG_TIMEOUT 100
+#define UBX_CONFIG_TIMEOUT		200		// ms, timeout for waiting ACK
+#define UBX_PACKET_TIMEOUT		2		// ms, if now data during this delay assume that full update received
+#define UBX_WAIT_BEFORE_READ	20		// ms, wait before reading to save read() calls
 
 UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) :
-_fd(fd),
-_gps_position(gps_position),
-_waiting_for_ack(false),
-_disable_cmd_counter(0)
+	_fd(fd),
+	_gps_position(gps_position),
+	_configured(false),
+	_waiting_for_ack(false),
+	_disable_cmd_counter(0)
 {
 	decode_init();
 }
@@ -73,12 +77,13 @@ UBX::~UBX()
 int
 UBX::configure(unsigned &baudrate)
 {
-	_waiting_for_ack = true;
-
+	_configured = false;
 	/* try different baudrates */
 	const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200};
 
-	for (int baud_i = 0; baud_i < 5; baud_i++) {
+	int baud_i;
+
+	for (baud_i = 0; baud_i < 5; baud_i++) {
 		baudrate = baudrates_to_try[baud_i];
 		set_baudrate(_fd, baudrate);
 
@@ -89,8 +94,8 @@ UBX::configure(unsigned &baudrate)
 		/* Set everything else of the packet to 0, otherwise the module wont accept it */
 		memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet));
 
-		_clsID_needed = UBX_CLASS_CFG;
-		_msgID_needed = UBX_MESSAGE_CFG_PRT;
+		_message_class_needed = UBX_CLASS_CFG;
+		_message_id_needed = UBX_MESSAGE_CFG_PRT;
 
 		/* Define the package contents, don't change the baudrate */
 		cfg_prt_packet.clsID		= UBX_CLASS_CFG;
@@ -102,9 +107,9 @@ UBX::configure(unsigned &baudrate)
 		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
 		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
 
-		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
+		send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet));
 
-		if (receive(UBX_CONFIG_TIMEOUT) < 0) {
+		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
 			/* try next baudrate */
 			continue;
 		}
@@ -120,100 +125,125 @@ UBX::configure(unsigned &baudrate)
 		cfg_prt_packet.inProtoMask	= UBX_CFG_PRT_PAYLOAD_INPROTOMASK;
 		cfg_prt_packet.outProtoMask	= UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK;
 
-		send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet));
-		
+		send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet));
+
 		/* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */
-		receive(UBX_CONFIG_TIMEOUT);
-		
+		wait_for_ack(UBX_CONFIG_TIMEOUT);
+
 		if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) {
 			set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE);
 			baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE;
 		}
 
-		/* send a CFT-RATE message to define update rate */
-		type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
-		memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
+		/* at this point we have correct baudrate on both ends */
+		break;
+	}
 
-		_clsID_needed = UBX_CLASS_CFG;
-		_msgID_needed = UBX_MESSAGE_CFG_RATE;
+	if (baud_i >= 5) {
+		return 1;
+	}
 
-		cfg_rate_packet.clsID		= UBX_CLASS_CFG;
-		cfg_rate_packet.msgID		= UBX_MESSAGE_CFG_RATE;
-		cfg_rate_packet.length		= UBX_CFG_RATE_LENGTH;
-		cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASINTERVAL;
-		cfg_rate_packet.navRate		= UBX_CFG_RATE_PAYLOAD_NAVRATE;
-		cfg_rate_packet.timeRef		= UBX_CFG_RATE_PAYLOAD_TIMEREF;
+	/* send a CFG-RATE message to define update rate */
+	type_gps_bin_cfg_rate_packet_t cfg_rate_packet;
+	memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet));
 
-		send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet));
-		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
+	_message_class_needed = UBX_CLASS_CFG;
+	_message_id_needed = UBX_MESSAGE_CFG_RATE;
 
-		/* send a NAV5 message to set the options for the internal filter */
-		type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
-		memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
+	cfg_rate_packet.clsID		= UBX_CLASS_CFG;
+	cfg_rate_packet.msgID		= UBX_MESSAGE_CFG_RATE;
+	cfg_rate_packet.length		= UBX_CFG_RATE_LENGTH;
+	cfg_rate_packet.measRate	= UBX_CFG_RATE_PAYLOAD_MEASINTERVAL;
+	cfg_rate_packet.navRate		= UBX_CFG_RATE_PAYLOAD_NAVRATE;
+	cfg_rate_packet.timeRef		= UBX_CFG_RATE_PAYLOAD_TIMEREF;
 
-		_clsID_needed = UBX_CLASS_CFG;
-		_msgID_needed = UBX_MESSAGE_CFG_NAV5;
+	send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet));
 
-		cfg_nav5_packet.clsID        = UBX_CLASS_CFG;
-		cfg_nav5_packet.msgID        = UBX_MESSAGE_CFG_NAV5;
-		cfg_nav5_packet.length       = UBX_CFG_NAV5_LENGTH;
-		cfg_nav5_packet.mask         = UBX_CFG_NAV5_PAYLOAD_MASK;
-		cfg_nav5_packet.dynModel     = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
-		cfg_nav5_packet.fixMode      = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("ubx: configuration failed: RATE");
+		return 1;
+	}
 
-		send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
-		if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
-			/* try next baudrate */
-			continue;
-		}
+	/* send a NAV5 message to set the options for the internal filter */
+	type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet;
+	memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet));
 
-		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH,
-			UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
-		/* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC,
-			UBX_CFG_MSG_PAYLOAD_RATE1_1HZ);
-		// /* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL,
-			UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
-		// /* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED,
-			UBX_CFG_MSG_PAYLOAD_RATE1_5HZ);
-		// /* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-		// configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP,
-		// 	0);
-		// /* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-		configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO,
-			UBX_CFG_MSG_PAYLOAD_RATE1_05HZ);
-		// /* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-
-		_waiting_for_ack = false;
-		return 0;
+	_message_class_needed = UBX_CLASS_CFG;
+	_message_id_needed = UBX_MESSAGE_CFG_NAV5;
+
+	cfg_nav5_packet.clsID        = UBX_CLASS_CFG;
+	cfg_nav5_packet.msgID        = UBX_MESSAGE_CFG_NAV5;
+	cfg_nav5_packet.length       = UBX_CFG_NAV5_LENGTH;
+	cfg_nav5_packet.mask         = UBX_CFG_NAV5_PAYLOAD_MASK;
+	cfg_nav5_packet.dynModel     = UBX_CFG_NAV5_PAYLOAD_DYNMODEL;
+	cfg_nav5_packet.fixMode      = UBX_CFG_NAV5_PAYLOAD_FIXMODE;
+
+	send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet));
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("ubx: configuration failed: NAV5");
+		return 1;
 	}
-	return -1;
+
+	/* configure message rates */
+	/* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */
+	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1);
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("ubx: msg rate configuration failed: NAV POSLLH\n");
+		return 1;
+	}
+
+	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1);
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n");
+		return 1;
+	}
+
+	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1);
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("ubx: msg rate configuration failed: NAV SOL\n");
+		return 1;
+	}
+
+	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1);
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("ubx: msg rate configuration failed: NAV VELNED\n");
+		return 1;
+	}
+
+	configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5);
+
+	if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) {
+		warnx("ubx: msg rate configuration failed: NAV SVINFO\n");
+		return 1;
+	}
+
+	_configured = true;
+	return 0;
 }
 
 int
 UBX::wait_for_ack(unsigned timeout)
 {
 	_waiting_for_ack = true;
-	int ret = receive(timeout);
-	_waiting_for_ack = false;
-	return ret;
+	uint64_t time_started = hrt_absolute_time();
+
+	while (hrt_absolute_time() < time_started + timeout * 1000) {
+		if (receive(timeout) > 0) {
+			if (!_waiting_for_ack) {
+				return 1;
+			}
+
+		} else {
+			return -1;	// timeout or error receiving, or NAK
+		}
+	}
+
+	return -1;	// timeout
 }
 
 int
@@ -231,48 +261,47 @@ UBX::receive(unsigned timeout)
 
 	ssize_t count = 0;
 
-	bool position_updated = false;
+	bool handled = false;
 
 	while (true) {
 
-		/* poll for new data */
-		int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout);
+		/* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */
+		int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout);
 
 		if (ret < 0) {
 			/* something went wrong when polling */
 			return -1;
 
 		} else if (ret == 0) {
-			/* Timeout */
-			return -1;
+			/* return success after short delay after receiving a packet or timeout after long delay */
+			return handled ? 1 : -1;
 
 		} else if (ret > 0) {
 			/* if we have new data from GPS, go handle it */
 			if (fds[0].revents & POLLIN) {
 				/*
 				 * We are here because poll says there is some data, so this
-				 * won't block even on a blocking device.  If more bytes are
-				 * available, we'll go back to poll() again...
+				 * won't block even on a blocking device. But don't read immediately
+				 * by 1-2 bytes, wait for some more data to save expensive read() calls.
+				 * If more bytes are available, we'll go back to poll() again.
 				 */
-				count = ::read(_fd, buf, sizeof(buf));
-						/* pass received bytes to the packet decoder */
+				usleep(UBX_WAIT_BEFORE_READ * 1000);
+				count = read(_fd, buf, sizeof(buf));
+
+				/* pass received bytes to the packet decoder */
 				for (int i = 0; i < count; i++) {
-					if (parse_char(buf[i])) {
+					if (parse_char(buf[i]) > 0) {
 						/* return to configure during configuration or to the gps driver during normal work
 						 * if a packet has arrived */
-						if (handle_message())
-							position_updated = true;
+						if (handle_message() > 0)
+							handled = true;
 					}
 				}
 			}
 		}
 
-		/* return success after receiving a packet */
-		if (position_updated)
-			return 1;
-
-		/* abort after timeout if no packet parsed successfully */
-		if (time_started + timeout*1000 < hrt_absolute_time() ) {
+		/* abort after timeout if no useful packets received */
+		if (time_started + timeout * 1000 < hrt_absolute_time()) {
 			return -1;
 		}
 	}
@@ -283,406 +312,292 @@ UBX::parse_char(uint8_t b)
 {
 	switch (_decode_state) {
 		/* First, look for sync1 */
-		case UBX_DECODE_UNINIT:
-			if (b == UBX_SYNC1) {
-				_decode_state = UBX_DECODE_GOT_SYNC1;
-			}
-			break;
+	case UBX_DECODE_UNINIT:
+		if (b == UBX_SYNC1) {
+			_decode_state = UBX_DECODE_GOT_SYNC1;
+		}
+
+		break;
+
 		/* Second, look for sync2 */
-		case UBX_DECODE_GOT_SYNC1:
-			if (b == UBX_SYNC2) {
-				_decode_state = UBX_DECODE_GOT_SYNC2;
-			} else {
-				/* Second start symbol was wrong, reset state machine */
-				decode_init();
-			}
-			break;
+	case UBX_DECODE_GOT_SYNC1:
+		if (b == UBX_SYNC2) {
+			_decode_state = UBX_DECODE_GOT_SYNC2;
+
+		} else {
+			/* Second start symbol was wrong, reset state machine */
+			decode_init();
+			/* don't return error, it can be just false sync1 */
+		}
+
+		break;
+
 		/* Now look for class */
-		case UBX_DECODE_GOT_SYNC2:
-			/* everything except sync1 and sync2 needs to be added to the checksum */
-			add_byte_to_checksum(b);
-			/* check for known class */
-			switch (b) {
-				case UBX_CLASS_ACK:
-					_decode_state = UBX_DECODE_GOT_CLASS;
-					_message_class = ACK;
-					break;
+	case UBX_DECODE_GOT_SYNC2:
+		/* everything except sync1 and sync2 needs to be added to the checksum */
+		add_byte_to_checksum(b);
+		_message_class = b;
+		_decode_state = UBX_DECODE_GOT_CLASS;
+		break;
 
-				case UBX_CLASS_NAV:
-					_decode_state = UBX_DECODE_GOT_CLASS;
-					_message_class = NAV;
-					break;
+	case UBX_DECODE_GOT_CLASS:
+		add_byte_to_checksum(b);
+		_message_id = b;
+		_decode_state = UBX_DECODE_GOT_MESSAGEID;
+		break;
 
-//				case UBX_CLASS_RXM:
-//					_decode_state = UBX_DECODE_GOT_CLASS;
-//					_message_class = RXM;
-//					break;
+	case UBX_DECODE_GOT_MESSAGEID:
+		add_byte_to_checksum(b);
+		_payload_size = b; //this is the first length byte
+		_decode_state = UBX_DECODE_GOT_LENGTH1;
+		break;
 
-				case UBX_CLASS_CFG:
-					_decode_state = UBX_DECODE_GOT_CLASS;
-					_message_class = CFG;
-					break;
-				default: //unknown class: reset state machine
-					decode_init();
-					break;
-			}
-			break;
-		case UBX_DECODE_GOT_CLASS:
-			{
+	case UBX_DECODE_GOT_LENGTH1:
+		add_byte_to_checksum(b);
+		_payload_size += b << 8; // here comes the second byte of length
+		_decode_state = UBX_DECODE_GOT_LENGTH2;
+		break;
+
+	case UBX_DECODE_GOT_LENGTH2:
+
+		/* Add to checksum if not yet at checksum byte */
+		if (_rx_count < _payload_size)
 			add_byte_to_checksum(b);
-			switch (_message_class) {
-				case NAV:
-					switch (b) {
-					case UBX_MESSAGE_NAV_POSLLH:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_POSLLH;
-						break;
-
-					case UBX_MESSAGE_NAV_SOL:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_SOL;
-						break;
-
-					case UBX_MESSAGE_NAV_TIMEUTC:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_TIMEUTC;
-						break;
-
-//					case UBX_MESSAGE_NAV_DOP:
-//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-//						_message_id = NAV_DOP;
-//						break;
-
-					case UBX_MESSAGE_NAV_SVINFO:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_SVINFO;
-						break;
-
-					case UBX_MESSAGE_NAV_VELNED:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_VELNED;
-						break;
-
-					default: //unknown class: reset state machine, should not happen
-						decode_init();
-						break;
-					}
-					break;
-//				case RXM:
-//					switch (b) {
-//					case UBX_MESSAGE_RXM_SVSI:
-//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-//						_message_id = RXM_SVSI;
-//						break;
-//
-//					default: //unknown class: reset state machine, should not happen
-//						decode_init();
-//						break;
-//					}
-//					break;
-
-				case CFG:
-					switch (b) {
-					case UBX_MESSAGE_CFG_NAV5:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = CFG_NAV5;
-						break;
-
-					default: //unknown class: reset state machine, should not happen
-						decode_init();
-						break;
-					}
-					break;
 
-				case ACK:
-					switch (b) {
-					case UBX_MESSAGE_ACK_ACK:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = ACK_ACK;
-						break;
-					case UBX_MESSAGE_ACK_NAK:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = ACK_NAK;
-						break;
-					default: //unknown class: reset state machine, should not happen
-						decode_init();
-						break;
-					}
-					break;
-				default: //should not happen because we set the class
-					warnx("UBX Error, we set a class that we don't know");
-					decode_init();
-//					config_needed = true;
-					break;
-				}
-				// Evaluate state machine - if the state changed,
-				// the state machine was reset via decode_init()
-				// and we want to tell the module to stop sending this message
+		_rx_buffer[_rx_count] = b;
 
-				// disable unknown message
-				//warnx("disabled class %d, msg %d", (int)_message_class, (int)b);
-				//configure_message_rate(_message_class, b, 0);
+		/* once the payload has arrived, we can process the information */
+		if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
+			/* compare checksum */
+			if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) {
+				decode_init();
+				return 1;	// message received successfully
+
+			} else {
+				warnx("ubx: checksum wrong");
+				decode_init();
+				return -1;
 			}
-				break;
-			case UBX_DECODE_GOT_MESSAGEID:
-				add_byte_to_checksum(b);
-				_payload_size = b; //this is the first length byte
-				_decode_state = UBX_DECODE_GOT_LENGTH1;
-				break;
-			case UBX_DECODE_GOT_LENGTH1:
-				add_byte_to_checksum(b);
-				_payload_size += b << 8; // here comes the second byte of length
-				_decode_state = UBX_DECODE_GOT_LENGTH2;
-				break;
-			case UBX_DECODE_GOT_LENGTH2:
-				/* Add to checksum if not yet at checksum byte */
-				if (_rx_count < _payload_size)
-					add_byte_to_checksum(b);
-					_rx_buffer[_rx_count] = b;
-					/* once the payload has arrived, we can process the information */
-					if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes
-
-						/* compare checksum */
-						if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) {
-							return 1;
-						} else {
-							decode_init();
-							return -1;
-							warnx("ubx: Checksum wrong");
-						}
 
-						return 1;
-					} else if (_rx_count < RECV_BUFFER_SIZE) {
-						_rx_count++;
-					} else {
-						warnx("ubx: buffer full");
-						decode_init();
-						return -1;
-					}
-				break;
-		default:
-			break;
+		} else if (_rx_count < RECV_BUFFER_SIZE) {
+			_rx_count++;
+
+		} else {
+			warnx("ubx: buffer full");
+			decode_init();
+			return -1;
+		}
+
+		break;
+
+	default:
+		break;
 	}
-	return 0; //XXX ?
+
+	return 0;	// message decoding in progress
 }
 
 
+
 int
 UBX::handle_message()
 {
 	int ret = 0;
 
-	switch (_message_id) { //this enum is unique for all ids --> no need to check the class
-		case NAV_POSLLH: {
-//			printf("GOT NAV_POSLLH MESSAGE\n");
-			if (!_waiting_for_ack) {
-				gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
+	if (_configured) {
+		/* handle only info messages when configured */
+		switch (_message_class) {
+		case UBX_CLASS_NAV:
+			switch (_message_id) {
+			case UBX_MESSAGE_NAV_POSLLH: {
+					// printf("GOT NAV_POSLLH\n");
+					gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer;
 
-				_gps_position->lat = packet->lat;
-				_gps_position->lon = packet->lon;
-				_gps_position->alt = packet->height_msl;
+					_gps_position->lat = packet->lat;
+					_gps_position->lon = packet->lon;
+					_gps_position->alt = packet->height_msl;
+					_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
+					_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
+					_gps_position->timestamp_position = hrt_absolute_time();
 
-				_gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m
-				_gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m
+					_rate_count_lat_lon++;
 
-				_rate_count_lat_lon++;
+					ret = 1;
+					break;
+				}
 
-				/* Add timestamp to finish the report */
-				_gps_position->timestamp_position = hrt_absolute_time();
-				/* only return 1 when new position is available */
-				ret = 1;
-			}
-			break;
-		}
+			case UBX_MESSAGE_NAV_SOL: {
+					// printf("GOT NAV_SOL\n");
+					gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
 
-		case NAV_SOL: {
-//			printf("GOT NAV_SOL MESSAGE\n");
-			if (!_waiting_for_ack) {
-				gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer;
+					_gps_position->fix_type = packet->gpsFix;
+					_gps_position->s_variance_m_s = packet->sAcc;
+					_gps_position->p_variance_m = packet->pAcc;
+					_gps_position->timestamp_variance = hrt_absolute_time();
 
-				_gps_position->fix_type = packet->gpsFix;
-				_gps_position->s_variance_m_s = packet->sAcc;
-				_gps_position->p_variance_m = packet->pAcc;
+					ret = 1;
+					break;
+				}
 
-				_gps_position->timestamp_variance = hrt_absolute_time();
-			}
-			break;
-		}
+			case UBX_MESSAGE_NAV_TIMEUTC: {
+					// printf("GOT NAV_TIMEUTC\n");
+					gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
 
-//		case NAV_DOP: {
-////		printf("GOT NAV_DOP MESSAGE\n");
-//			gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
-//
-//			_gps_position->eph_m =  packet->hDOP;
-//			_gps_position->epv =  packet->vDOP;
-//
-//			_gps_position->timestamp_posdilution = hrt_absolute_time();
-//
-//			_new_nav_dop = true;
-//
-//			break;
-//		}
-
-		case NAV_TIMEUTC: {
-//			printf("GOT NAV_TIMEUTC MESSAGE\n");
+					/* convert to unix timestamp */
+					struct tm timeinfo;
+					timeinfo.tm_year = packet->year - 1900;
+					timeinfo.tm_mon = packet->month - 1;
+					timeinfo.tm_mday = packet->day;
+					timeinfo.tm_hour = packet->hour;
+					timeinfo.tm_min = packet->min;
+					timeinfo.tm_sec = packet->sec;
+					time_t epoch = mktime(&timeinfo);
 
-			if (!_waiting_for_ack) {
-				gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
+					_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
+					_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
+					_gps_position->timestamp_time = hrt_absolute_time();
 
-				//convert to unix timestamp
-				struct tm timeinfo;
-				timeinfo.tm_year = packet->year - 1900;
-				timeinfo.tm_mon = packet->month - 1;
-				timeinfo.tm_mday = packet->day;
-				timeinfo.tm_hour = packet->hour;
-				timeinfo.tm_min = packet->min;
-				timeinfo.tm_sec = packet->sec;
+					ret = 1;
+					break;
+				}
 
-				time_t epoch = mktime(&timeinfo);
+			case UBX_MESSAGE_NAV_SVINFO: {
+					//printf("GOT NAV_SVINFO\n");
+					const int length_part1 = 8;
+					gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer;
+					const int length_part2 = 12;
+					gps_bin_nav_svinfo_part2_packet_t *packet_part2;
 
-				_gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
-				_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
+					uint8_t satellites_used = 0;
+					int i;
 
-				_gps_position->timestamp_time = hrt_absolute_time();
-			}
-			break;
-		}
+					//printf("Number of Channels: %d\n", packet_part1->numCh);
+					for (i = 0; i < packet_part1->numCh; i++) {
+						/* set pointer to sattelite_i information */
+						packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]);
 
-		case NAV_SVINFO: {
-			// printf("GOT NAV_SVINFO MESSAGE\n");
+						/* write satellite information to global storage */
+						uint8_t sv_used = packet_part2->flags & 0x01;
 
-			if (!_waiting_for_ack) {
-				//this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message
-				const int length_part1 = 8;
-				char _rx_buffer_part1[length_part1];
-				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
-				gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1;
-
-				//read checksum
-				const int length_part3 = 2;
-				char _rx_buffer_part3[length_part3];
-				memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3);
-
-				//definitions needed to read numCh elements from the buffer:
-				const int length_part2 = 12;
-				gps_bin_nav_svinfo_part2_packet_t *packet_part2;
-				char _rx_buffer_part2[length_part2]; //for temporal storage
-
-				uint8_t satellites_used = 0;
-				int i;
-				// printf("Number of Channels: %d\n", packet_part1->numCh);
-				for (i = 0; i < packet_part1->numCh; i++) { //for each channel
-
-					/* Get satellite information from the buffer */
-					memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2);
-					packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2;
-
-					/* Write satellite information to global storage */
-					uint8_t sv_used = packet_part2->flags & 0x01;
-
-					if ( sv_used ) {
-						// Count SVs used for NAV. 
-						satellites_used++;
+						if (sv_used) {
+							/* count SVs used for NAV */
+							satellites_used++;
+						}
+
+						/* record info for all channels, whether or not the SV is used for NAV */
+						_gps_position->satellite_used[i] = sv_used;
+						_gps_position->satellite_snr[i] = packet_part2->cno;
+						_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
+						_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
+						_gps_position->satellite_prn[i] = packet_part2->svid;
+						//printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid);
+					}
+
+					for (i = packet_part1->numCh; i < 20; i++) {
+						/* unused channels have to be set to zero for e.g. MAVLink */
+						_gps_position->satellite_prn[i] = 0;
+						_gps_position->satellite_used[i] = 0;
+						_gps_position->satellite_snr[i] = 0;
+						_gps_position->satellite_elevation[i] = 0;
+						_gps_position->satellite_azimuth[i] = 0;
+					}
+
+					_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
+
+					if (packet_part1->numCh > 0) {
+						_gps_position->satellite_info_available = true;
+
+					} else {
+						_gps_position->satellite_info_available = false;
 					}
-		
-					// Record info for all channels, whether or not the SV is used for NAV.
-					_gps_position->satellite_used[i] = sv_used;
-					_gps_position->satellite_snr[i] = packet_part2->cno;
-					_gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev);
-					_gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f);
-					_gps_position->satellite_prn[i] = packet_part2->svid; 
-				}
 
-				for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused
-					/* Unused channels have to be set to zero for e.g. MAVLink */
-					_gps_position->satellite_prn[i] = 0;
-					_gps_position->satellite_used[i] = 0;
-					_gps_position->satellite_snr[i] = 0;
-					_gps_position->satellite_elevation[i] = 0;
-					_gps_position->satellite_azimuth[i] = 0;
+					_gps_position->timestamp_satellites = hrt_absolute_time();
+
+					ret = 1;
+					break;
 				}
-				_gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones
 
-				/* set timestamp if any sat info is available */
-				if (packet_part1->numCh > 0) {
-					_gps_position->satellite_info_available = true;
-				} else {
-					_gps_position->satellite_info_available = false;
+			case UBX_MESSAGE_NAV_VELNED: {
+					// printf("GOT NAV_VELNED\n");
+					gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
+
+					_gps_position->vel_m_s   = (float)packet->speed * 1e-2f;
+					_gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */
+					_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */
+					_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */
+					_gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
+					_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
+					_gps_position->vel_ned_valid = true;
+					_gps_position->timestamp_velocity = hrt_absolute_time();
+
+					_rate_count_vel++;
+
+					ret = 1;
+					break;
 				}
-				_gps_position->timestamp_satellites = hrt_absolute_time();
+
+			default:
+				break;
 			}
 
 			break;
-		}
 
-		case NAV_VELNED: {
-
-			if (!_waiting_for_ack) {
-				/* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */
-				gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
-
-				_gps_position->vel_m_s   = (float)packet->speed * 1e-2f;
-				_gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */
-				_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */
-				_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */
-				_gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
-				_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
-				_gps_position->vel_ned_valid = true;
-				_gps_position->timestamp_velocity = hrt_absolute_time();
-
-				_rate_count_vel++;
+		case UBX_CLASS_ACK: {
+				/* ignore ACK when already configured */
+				ret = 1;
+				break;
 			}
 
+		default:
 			break;
 		}
 
-//		case RXM_SVSI: {
-//				printf("GOT RXM_SVSI MESSAGE\n");
-//				const int length_part1 = 7;
-//				char _rx_buffer_part1[length_part1];
-//				memcpy(_rx_buffer_part1, _rx_buffer, length_part1);
-//				gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1;
-//
-//				_gps_position->satellites_visible = packet->numVis;
-//				_gps_position->counter++;
-//				_last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time();
-//
-//			break;
-//		}
-		case ACK_ACK: {
-//			printf("GOT ACK_ACK\n");
-			gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer;
-
-			if (_waiting_for_ack) {
-				if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) {
-					ret = 1;
-				}
+		if (ret == 0) {
+			/* message not handled */
+			warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id);
+
+			if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) {
+				/* don't attempt for every message to disable, some might not be disabled */
+				warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id);
+				configure_message_rate(_message_class, _message_id, 0);
 			}
 		}
-		break;
 
-		case ACK_NAK: {
-//			printf("GOT ACK_NAK\n");
-			warnx("UBX: Received: Not Acknowledged");
-			/* configuration obviously not successful */
-			ret = -1;
-			break;
-		}
+	} else {
+		/* handle only ACK while configuring */
+		if (_message_class == UBX_CLASS_ACK) {
+			switch (_message_id) {
+			case UBX_MESSAGE_ACK_ACK: {
+					// printf("GOT ACK_ACK\n");
+					gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer;
+
+					if (_waiting_for_ack) {
+						if (packet->clsID == _message_class_needed && packet->msgID == _message_id_needed) {
+							_waiting_for_ack = false;
+							ret = 1;
+						}
+					}
 
-		default: //we don't know the message
-			warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id);
-			if (_disable_cmd_counter++ == 0) {
-				// Don't attempt for every message to disable, some might not be disabled */
-				warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id);
-				configure_message_rate(_message_class, _message_id, 0);
+					break;
+				}
+
+			case UBX_MESSAGE_ACK_NAK: {
+					// printf("GOT ACK_NAK\n");
+					warnx("ubx: not acknowledged");
+					/* configuration obviously not successful */
+					_waiting_for_ack = false;
+					ret = -1;
+					break;
+				}
+
+			default:
+				break;
 			}
-		return ret;
-			ret = -1;
-			break;
 		}
-	// end if _rx_count high enough
+	}
+
 	decode_init();
-	return ret; //XXX?
+	return ret;
 }
 
 void
@@ -692,9 +607,8 @@ UBX::decode_init(void)
 	_rx_ck_b = 0;
 	_rx_count = 0;
 	_decode_state = UBX_DECODE_UNINIT;
-	_message_class = CLASS_UNKNOWN;
-	_message_id = ID_UNKNOWN;
 	_payload_size = 0;
+	/* don't reset _message_class, _message_id, _rx_buffer leave it for message handler */
 }
 
 void
@@ -705,23 +619,24 @@ UBX::add_byte_to_checksum(uint8_t b)
 }
 
 void
-UBX::add_checksum_to_message(uint8_t* message, const unsigned length)
+UBX::add_checksum_to_message(uint8_t *message, const unsigned length)
 {
 	uint8_t ck_a = 0;
 	uint8_t ck_b = 0;
 	unsigned i;
 
-	for (i = 0; i < length-2; i++) {
+	for (i = 0; i < length - 2; i++) {
 		ck_a = ck_a + message[i];
 		ck_b = ck_b + ck_a;
 	}
+
 	/* The checksum is written to the last to bytes of a message */
-	message[length-2] = ck_a;
-	message[length-1] = ck_b;
+	message[length - 2] = ck_a;
+	message[length - 1] = ck_b;
 }
 
 void
-UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
+UBX::add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b)
 {
 	for (unsigned i = 0; i < length; i++) {
 		ck_a = ck_a + message[i];
@@ -732,11 +647,11 @@ UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_
 void
 UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate)
 {
-    struct ubx_cfg_msg_rate msg;
-    msg.msg_class	= msg_class;
-    msg.msg_id		= msg_id;
-    msg.rate		= rate;
-    send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
+	struct ubx_cfg_msg_rate msg;
+	msg.msg_class	= msg_class;
+	msg.msg_id		= msg_id;
+	msg.rate		= rate;
+	send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg));
 }
 
 void
@@ -761,19 +676,19 @@ void
 UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size)
 {
 	struct ubx_header header;
-	uint8_t ck_a=0, ck_b=0;
+	uint8_t ck_a = 0, ck_b = 0;
 	header.sync1 = UBX_SYNC1;
 	header.sync2 = UBX_SYNC2;
 	header.msg_class = msg_class;
 	header.msg_id    = msg_id;
 	header.length    = size;
 
-	add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b);
+	add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b);
 	add_checksum((uint8_t *)msg, size, ck_a, ck_b);
 
 	// Configure receive check
-	_clsID_needed = msg_class;
-	_msgID_needed = msg_id;
+	_message_class_needed = msg_class;
+	_message_id_needed = msg_id;
 
 	write(_fd, (const char *)&header, sizeof(header));
 	write(_fd, (const char *)msg, size);
diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h
index 5a433642c..4fc276975 100644
--- a/src/drivers/gps/ubx.h
+++ b/src/drivers/gps/ubx.h
@@ -3,6 +3,7 @@
  *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
  *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
  *           Julian Oes <joes@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -51,23 +52,23 @@
 
 /* MessageIDs (the ones that are used) */
 #define UBX_MESSAGE_NAV_POSLLH 0x02
-#define UBX_MESSAGE_NAV_SOL 0x06
-#define UBX_MESSAGE_NAV_TIMEUTC 0x21
 //#define UBX_MESSAGE_NAV_DOP 0x04
-#define UBX_MESSAGE_NAV_SVINFO 0x30
+#define UBX_MESSAGE_NAV_SOL 0x06
 #define UBX_MESSAGE_NAV_VELNED 0x12
 //#define UBX_MESSAGE_RXM_SVSI 0x20
-#define UBX_MESSAGE_ACK_ACK 0x01
+#define UBX_MESSAGE_NAV_TIMEUTC 0x21
+#define UBX_MESSAGE_NAV_SVINFO 0x30
 #define UBX_MESSAGE_ACK_NAK 0x00
+#define UBX_MESSAGE_ACK_ACK 0x01
 #define UBX_MESSAGE_CFG_PRT 0x00
-#define UBX_MESSAGE_CFG_NAV5 0x24
 #define UBX_MESSAGE_CFG_MSG 0x01
 #define UBX_MESSAGE_CFG_RATE 0x08
+#define UBX_MESSAGE_CFG_NAV5 0x24
 
 #define UBX_CFG_PRT_LENGTH 20
 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01			/**< UART1 */
 #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0		/**< 0b0000100011010000: 8N1 */
-#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400		/**< always choose 38400 as GPS baudrate */
+#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400		/**< choose 38400 as GPS baudrate */
 #define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01		/**< UBX in */
 #define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01		/**< UBX out */
 
@@ -299,44 +300,6 @@ struct ubx_cfg_msg_rate {
 // ************
 
 typedef enum {
-	UBX_CONFIG_STATE_PRT = 0,
-	UBX_CONFIG_STATE_PRT_NEW_BAUDRATE,
-	UBX_CONFIG_STATE_RATE,
-	UBX_CONFIG_STATE_NAV5,
-	UBX_CONFIG_STATE_MSG_NAV_POSLLH,
-	UBX_CONFIG_STATE_MSG_NAV_TIMEUTC,
-	UBX_CONFIG_STATE_MSG_NAV_DOP,
-	UBX_CONFIG_STATE_MSG_NAV_SVINFO,
-	UBX_CONFIG_STATE_MSG_NAV_SOL,
-	UBX_CONFIG_STATE_MSG_NAV_VELNED,
-//	UBX_CONFIG_STATE_MSG_RXM_SVSI,
-	UBX_CONFIG_STATE_CONFIGURED
-} ubx_config_state_t;
-
-typedef enum {
-	CLASS_UNKNOWN = 0,
-	NAV = 1,
-	RXM = 2,
-	ACK = 3,
-	CFG = 4
-} ubx_message_class_t;
-
-typedef enum {
-	//these numbers do NOT correspond to the message id numbers of the ubx protocol
-	ID_UNKNOWN = 0,
-	NAV_POSLLH,
-	NAV_SOL,
-	NAV_TIMEUTC,
-//	NAV_DOP,
-	NAV_SVINFO,
-	NAV_VELNED,
-//	RXM_SVSI,
-	CFG_NAV5,
-	ACK_ACK,
-	ACK_NAK,
-} ubx_message_id_t;
-
-typedef enum {
 	UBX_DECODE_UNINIT = 0,
 	UBX_DECODE_GOT_SYNC1,
 	UBX_DECODE_GOT_SYNC2,
@@ -401,17 +364,17 @@ private:
 
 	int			_fd;
 	struct vehicle_gps_position_s *_gps_position;
-	ubx_config_state_t	_config_state;
+	bool			_configured;
 	bool			_waiting_for_ack;
-	uint8_t			_clsID_needed;
-	uint8_t			_msgID_needed;
+	uint8_t			_message_class_needed;
+	uint8_t			_message_id_needed;
 	ubx_decode_state_t	_decode_state;
 	uint8_t			_rx_buffer[RECV_BUFFER_SIZE];
 	unsigned		_rx_count;
 	uint8_t			_rx_ck_a;
 	uint8_t			_rx_ck_b;
-	ubx_message_class_t	_message_class;
-	ubx_message_id_t	_message_id;
+	uint8_t			_message_class;
+	uint8_t			_message_id;
 	unsigned		_payload_size;
 	uint8_t			_disable_cmd_counter;
 };
diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp
new file mode 100644
index 000000000..cb8bbba37
--- /dev/null
+++ b/src/drivers/hott/comms.cpp
@@ -0,0 +1,92 @@
+/****************************************************************************
+ *
+ *   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 comms.c
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ */
+
+#include "comms.h"
+
+#include <fcntl.h>
+#include <stdio.h>
+#include <sys/ioctl.h>
+#include <systemlib/err.h>
+#include <termios.h>
+
+int
+open_uart(const char *device)
+{
+	/* baud rate */
+	static const speed_t speed = B19200;
+
+	/* open uart */
+	const int uart = open(device, O_RDWR | O_NOCTTY);
+
+	if (uart < 0) {
+		err(1, "Error opening port: %s", device);
+	}
+	
+	/* Back up the original uart configuration to restore it after exit */	
+	int termios_state;
+	struct termios uart_config_original;
+	if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) {
+		close(uart);
+		err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
+	}
+
+	/* Fill the struct for the new configuration */
+	struct termios uart_config;
+	tcgetattr(uart, &uart_config);
+
+	/* Clear ONLCR flag (which appends a CR for every LF) */
+	uart_config.c_oflag &= ~ONLCR;
+
+	/* Set baud rate */
+	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
+		close(uart);
+		err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
+			 device, termios_state);
+	}
+
+	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
+		close(uart);
+		err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
+	}
+
+	/* Activate single wire mode */
+	ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
+
+	return uart;
+}
diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h
new file mode 100644
index 000000000..f5608122f
--- /dev/null
+++ b/src/drivers/hott/comms.h
@@ -0,0 +1,46 @@
+/****************************************************************************
+ *
+ *   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 comms.h
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ */
+
+
+#ifndef COMMS_H_
+#define COMMS_H
+
+int open_uart(const char *device);
+
+#endif /* COMMS_H_ */
diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp
new file mode 100644
index 000000000..e322c6349
--- /dev/null
+++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp
@@ -0,0 +1,238 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Simon Wilks <sjwilks@gmail.com>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file hott_sensors.c
+ * @author Simon Wilks <sjwilks@gmail.com>
+ *
+ * Graupner HoTT sensor driver implementation.
+ *
+ * Poll any sensors connected to the PX4 via the telemetry wire.
+ */
+
+#include <fcntl.h>
+#include <nuttx/config.h>
+#include <poll.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <string.h>
+#include <sys/ioctl.h>
+#include <unistd.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+
+#include "../comms.h"
+#include "../messages.h"
+
+#define DEFAULT_UART "/dev/ttyS0";		/**< USART1 */
+
+/* Oddly, ERROR is not defined for C++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
+static int thread_should_exit = false;		/**< Deamon exit flag */
+static int thread_running = false;		/**< Deamon status flag */
+static int deamon_task;				/**< Handle of deamon task / thread */
+static const char daemon_name[] = "hott_sensors";
+static const char commandline_usage[] = "usage: hott_sensors start|status|stop [-d <device>]";
+
+/**
+ * Deamon management function.
+ */
+extern "C" __EXPORT int hott_sensors_main(int argc, char *argv[]);
+
+/**
+ * Mainloop of daemon.
+ */
+int hott_sensors_thread_main(int argc, char *argv[]);
+
+static int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id);
+static int send_poll(int uart, uint8_t *buffer, size_t size);
+
+int
+send_poll(int uart, uint8_t *buffer, size_t size)
+{
+	for (size_t i = 0; i < size; i++) {
+		write(uart, &buffer[i], sizeof(buffer[i]));
+
+		/* Sleep before sending the next byte. */
+		usleep(POST_WRITE_DELAY_IN_USECS);
+	}
+
+	/* A hack the reads out what was written so the next read from the receiver doesn't get it. */
+	/* TODO: Fix this!! */
+	uint8_t dummy[size];
+	read(uart, &dummy, size);
+
+	return OK;
+}
+
+int
+recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id)
+{
+	static const int timeout_ms = 1000;
+	
+	struct pollfd fds;
+	fds.fd = uart;
+	fds.events = POLLIN;
+	
+	// XXX should this poll be inside the while loop???
+	if (poll(&fds, 1, timeout_ms) > 0) {
+		int i = 0;
+		bool stop_byte_read = false;
+		while (true)  {
+			read(uart, &buffer[i], sizeof(buffer[i]));
+
+			if (stop_byte_read) {
+				// XXX process checksum
+				*size = ++i;
+				return OK;
+			}
+			// XXX can some other field not have the STOP BYTE value?
+			if (buffer[i] == STOP_BYTE) {
+				*id = buffer[1];
+				stop_byte_read = true;
+			}
+			i++;
+		}
+	}
+	return ERROR;
+}
+
+int
+hott_sensors_thread_main(int argc, char *argv[])
+{
+	warnx("starting");
+
+	thread_running = true;
+
+	const char *device = DEFAULT_UART;
+
+	/* read commandline arguments */
+	for (int i = 0; i < argc && argv[i]; i++) {
+		if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set
+			if (argc > i + 1) {
+				device = argv[i + 1];
+
+			} else {
+				thread_running = false;
+				errx(1, "missing parameter to -d\n%s", commandline_usage);
+			}
+		}
+	}
+
+	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
+	const int uart = open_uart(device);
+	if (uart < 0) {
+		errx(1, "Failed opening HoTT UART, exiting.");
+		thread_running = false;
+	}
+
+	init_pub_messages();
+
+	uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
+	size_t size = 0;
+	uint8_t id = 0;
+	while (!thread_should_exit) {
+		// Currently we only support a General Air Module sensor.
+		build_gam_request(&buffer[0], &size);
+		send_poll(uart, buffer, size);
+
+		// The sensor will need a little time before it starts sending.
+		usleep(5000);
+
+		recv_data(uart, &buffer[0], &size, &id);
+
+		// Determine which moduel sent it and process accordingly.
+		if (id == GAM_SENSOR_ID) {
+			publish_gam_message(buffer);
+		} else {
+			warnx("Unknown sensor ID: %d", id);
+		}
+	}
+
+	warnx("exiting");
+	close(uart);
+	thread_running = false;
+
+	return 0;
+}
+
+/**
+ * Process command line arguments and start the daemon.
+ */
+int
+hott_sensors_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		errx(1, "missing command\n%s", commandline_usage);
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (thread_running) {
+			warnx("deamon already running");
+			exit(0);
+		}
+
+		thread_should_exit = false;
+		deamon_task = task_spawn_cmd(daemon_name,
+					     SCHED_DEFAULT,
+					     SCHED_PRIORITY_MAX - 40,
+					     1024,
+					     hott_sensors_thread_main,
+					     (argv) ? (const char **)&argv[2] : (const char **)NULL);
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		thread_should_exit = true;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (thread_running) {
+			warnx("daemon is running");
+
+		} else {
+			warnx("daemon not started");
+		}
+
+		exit(0);
+	}
+
+	errx(1, "unrecognized command\n%s", commandline_usage);
+}
diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk
new file mode 100644
index 000000000..b5f5762ba
--- /dev/null
+++ b/src/drivers/hott/hott_sensors/module.mk
@@ -0,0 +1,42 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Graupner HoTT Sensors application.
+#
+
+MODULE_COMMAND	= hott_sensors
+
+SRCS		= hott_sensors.cpp \
+		  ../messages.cpp \
+		  ../comms.cpp
diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
index 4699ce5bf..042d9f816 100644
--- a/src/drivers/hott_telemetry/hott_telemetry_main.c
+++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp
@@ -41,7 +41,6 @@
  * The HoTT receiver polls each device at a regular interval at which point
  * a data packet can be returned if necessary.
  *
- * TODO: Add support for at least the vario and GPS sensor data.
  */
 
 #include <fcntl.h>
@@ -50,13 +49,21 @@
 #include <stdlib.h>
 #include <stdio.h>
 #include <string.h>
-#include <termios.h>
 #include <sys/ioctl.h>
 #include <unistd.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
 
-#include "messages.h"
+#include "../comms.h"
+#include "../messages.h"
+
+#define DEFAULT_UART "/dev/ttyS0";		/**< USART1 */
+
+/* Oddly, ERROR is not defined for C++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
 
 static int thread_should_exit = false;		/**< Deamon exit flag */
 static int thread_running = false;		/**< Deamon status flag */
@@ -67,7 +74,7 @@ static const char commandline_usage[] = "usage: hott_telemetry start|status|stop
 /**
  * Deamon management function.
  */
-__EXPORT int hott_telemetry_main(int argc, char *argv[]);
+extern "C" __EXPORT int hott_telemetry_main(int argc, char *argv[]);
 
 /**
  * Mainloop of daemon.
@@ -77,60 +84,18 @@ int hott_telemetry_thread_main(int argc, char *argv[]);
 static int recv_req_id(int uart, uint8_t *id);
 static int send_data(int uart, uint8_t *buffer, size_t size);
 
-static int
-open_uart(const char *device, struct termios *uart_config_original)
-{
-	/* baud rate */
-	static const speed_t speed = B19200;
-
-	/* open uart */
-	const int uart = open(device, O_RDWR | O_NOCTTY);
-
-	if (uart < 0) {
-		err(1, "Error opening port: %s", device);
-	}
-	
-	/* Back up the original uart configuration to restore it after exit */	
-	int termios_state;
-	if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) {
-		close(uart);
-		err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state);
-	}
-
-	/* Fill the struct for the new configuration */
-	struct termios uart_config;
-	tcgetattr(uart, &uart_config);
-
-	/* Clear ONLCR flag (which appends a CR for every LF) */
-	uart_config.c_oflag &= ~ONLCR;
-
-	/* Set baud rate */
-	if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) {
-		close(uart);
-		err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)",
-			 device, termios_state);
-	}
-
-	if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) {
-		close(uart);
-		err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device);
-	}
-
-	/* Activate single wire mode */
-	ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED);
-
-	return uart;
-}
-
 int
 recv_req_id(int uart, uint8_t *id)
 {
 	static const int timeout_ms = 1000;  // TODO make it a define
-	struct pollfd fds[] = { { .fd = uart, .events = POLLIN } };
 
 	uint8_t mode;
+	
+	struct pollfd fds;
+	fds.fd = uart;
+	fds.events = POLLIN;
 
-	if (poll(fds, 1, timeout_ms) > 0) {
+	if (poll(&fds, 1, timeout_ms) > 0) {
 		/* Get the mode: binary or text  */
 		read(uart, &mode, sizeof(mode));
 
@@ -155,7 +120,6 @@ send_data(int uart, uint8_t *buffer, size_t size)
 	usleep(POST_READ_DELAY_IN_USECS);
 
 	uint16_t checksum = 0;
-
 	for (size_t i = 0; i < size; i++) {
 		if (i == size - 1) {
 			/* Set the checksum: the first uint8_t is taken as the checksum. */
@@ -186,7 +150,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
 
 	thread_running = true;
 
-	const char *device = "/dev/ttyS1";		/**< Default telemetry port: USART2 */
+	const char *device = DEFAULT_UART;
 
 	/* read commandline arguments */
 	for (int i = 0; i < argc && argv[i]; i++) {
@@ -202,22 +166,20 @@ hott_telemetry_thread_main(int argc, char *argv[])
 	}
 
 	/* enable UART, writes potentially an empty buffer, but multiplexing is disabled */
-	struct termios uart_config_original;
-	const int uart = open_uart(device, &uart_config_original);
-
+	const int uart = open_uart(device);
 	if (uart < 0) {
 		errx(1, "Failed opening HoTT UART, exiting.");
 		thread_running = false;
 	}
 
-	messages_init();
+	init_sub_messages();
 
-	uint8_t buffer[MESSAGE_BUFFER_SIZE];
+	uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE];
 	size_t size = 0;
 	uint8_t id = 0;
 	bool connected = true;
-
 	while (!thread_should_exit) {
+		// Listen for and serve poll from the receiver.
 		if (recv_req_id(uart, &id) == OK) {
 			if (!connected) {
 				connected = true;
@@ -228,7 +190,9 @@ hott_telemetry_thread_main(int argc, char *argv[])
 			case EAM_SENSOR_ID:
 				build_eam_response(buffer, &size);
 				break;
-
+			case GAM_SENSOR_ID:
+				build_gam_response(buffer, &size);
+				break;
 			case GPS_SENSOR_ID:
 				build_gps_response(buffer, &size);
 				break;
@@ -254,7 +218,7 @@ hott_telemetry_thread_main(int argc, char *argv[])
 }
 
 /**
- * Process command line arguments and tart the daemon.
+ * Process command line arguments and start the daemon.
  */
 int
 hott_telemetry_main(int argc, char *argv[])
diff --git a/src/drivers/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk
index def1d59e9..b19cbd14c 100644
--- a/src/drivers/hott_telemetry/module.mk
+++ b/src/drivers/hott/hott_telemetry/module.mk
@@ -32,10 +32,11 @@
 ############################################################################
 
 #
-# Graupner HoTT Telemetry application.
+# Graupner HoTT Telemetry applications.
 #
 
 MODULE_COMMAND	= hott_telemetry
 
-SRCS		= hott_telemetry_main.c \
-		  messages.c
+SRCS		= hott_telemetry.cpp \
+		  ../messages.cpp \
+		  ../comms.cpp
diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott/messages.cpp
index 0ce56acef..57c256339 100644
--- a/src/drivers/hott_telemetry/messages.c
+++ b/src/drivers/hott/messages.cpp
@@ -46,6 +46,7 @@
 #include <unistd.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/battery_status.h>
+#include <uORB/topics/esc_status.h>
 #include <uORB/topics/home_position.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_gps_position.h>
@@ -53,24 +54,75 @@
 /* The board is very roughly 5 deg warmer than the surrounding air */
 #define BOARD_TEMP_OFFSET_DEG 5
 
-static int battery_sub = -1;
-static int gps_sub = -1;
-static int home_sub = -1;
-static int sensor_sub = -1;
-static int airspeed_sub = -1;
+static int _battery_sub = -1;
+static int _gps_sub = -1;
+static int _home_sub = -1;
+static int _sensor_sub = -1;
+static int _airspeed_sub = -1;
+static int _esc_sub = -1;
 
-static bool home_position_set = false;
-static double home_lat = 0.0d;
-static double home_lon = 0.0d;
+static orb_advert_t _esc_pub;
+struct esc_status_s _esc;
+
+static bool _home_position_set = false;
+static double _home_lat = 0.0d;
+static double _home_lon = 0.0d;
 
 void 
-messages_init(void)
+init_sub_messages(void)
 {
-	battery_sub = orb_subscribe(ORB_ID(battery_status));
-	gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
-	home_sub = orb_subscribe(ORB_ID(home_position));
-	sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
-	airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_battery_sub = orb_subscribe(ORB_ID(battery_status));
+	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	_home_sub = orb_subscribe(ORB_ID(home_position));
+	_sensor_sub = orb_subscribe(ORB_ID(sensor_combined));
+	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	_esc_sub = orb_subscribe(ORB_ID(esc_status));
+}
+
+void 
+init_pub_messages(void)
+{
+	memset(&_esc, 0, sizeof(_esc));
+	_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
+}
+
+void
+build_gam_request(uint8_t *buffer, size_t *size)
+{
+	struct gam_module_poll_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.mode = BINARY_MODE_REQUEST_ID;
+	msg.id = GAM_SENSOR_ID;
+
+	memcpy(buffer, &msg, *size);
+}
+
+void
+publish_gam_message(const uint8_t *buffer)
+{
+	struct gam_module_msg msg;
+	size_t size = sizeof(msg);
+	memset(&msg, 0, size);
+	memcpy(&msg, buffer, size);
+
+	/* announce the esc if needed, just publish else */
+	if (_esc_pub > 0) {
+		orb_publish(ORB_ID(esc_status), _esc_pub, &_esc);
+	} else {
+		_esc_pub = orb_advertise(ORB_ID(esc_status), &_esc);
+	}
+
+	// Publish it.
+	_esc.esc_count = 1;
+	_esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM;
+
+	_esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT;
+	_esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10;
+	_esc.esc[0].esc_temperature = msg.temperature1 - 20; 
+	_esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff));
+	_esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff));
 }
 
 void 
@@ -79,12 +131,12 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	/* get a local copy of the current sensor values */
 	struct sensor_combined_s raw;
 	memset(&raw, 0, sizeof(raw));
-	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+	orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
 
 	/* get a local copy of the battery data */
 	struct battery_status_s battery;
 	memset(&battery, 0, sizeof(battery));
-	orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+	orb_copy(ORB_ID(battery_status), _battery_sub, &battery);
 
 	struct eam_module_msg msg;
 	*size = sizeof(msg);
@@ -92,7 +144,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
 
 	msg.start = START_BYTE;
 	msg.eam_sensor_id = EAM_SENSOR_ID;
-	msg.sensor_id = EAM_SENSOR_TEXT_ID;
+	msg.sensor_text_id = EAM_SENSOR_TEXT_ID;
 	
 	msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20);
 	msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG;
@@ -106,12 +158,46 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	/* get a local copy of the airspeed data */
 	struct airspeed_s airspeed;
 	memset(&airspeed, 0, sizeof(airspeed));
-	orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
+	orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
 
 	uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f);
 	msg.speed_L = (uint8_t)speed & 0xff;
 	msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
-    
+
+	msg.stop = STOP_BYTE;
+	memcpy(buffer, &msg, *size);
+}
+
+void 
+build_gam_response(uint8_t *buffer, size_t *size)
+{
+	/* get a local copy of the ESC Status values */
+	struct esc_status_s esc;
+	memset(&esc, 0, sizeof(esc));
+	orb_copy(ORB_ID(esc_status), _esc_sub, &esc);
+
+	struct gam_module_msg msg;
+	*size = sizeof(msg);
+	memset(&msg, 0, *size);
+
+	msg.start = START_BYTE;
+	msg.gam_sensor_id = GAM_SENSOR_ID;
+	msg.sensor_text_id = GAM_SENSOR_TEXT_ID;
+	
+	msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20);
+	msg.temperature2 = 20;  // 0 deg. C.
+
+	uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage);
+	msg.main_voltage_L = (uint8_t)voltage & 0xff;
+	msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff;
+
+	uint16_t current = (uint16_t)(esc.esc[0].esc_current);
+	msg.current_L = (uint8_t)current & 0xff;
+	msg.current_H = (uint8_t)(current >> 8) & 0xff;
+
+	uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f);
+	msg.rpm_L = (uint8_t)rpm & 0xff;
+	msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff;
 
 	msg.stop = STOP_BYTE;
 	memcpy(buffer, &msg, *size);
@@ -123,14 +209,14 @@ build_gps_response(uint8_t *buffer, size_t *size)
 	/* get a local copy of the current sensor values */
 	struct sensor_combined_s raw;
 	memset(&raw, 0, sizeof(raw));
-	orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw);
+	orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw);
 
  	/* get a local copy of the battery data */
 	struct vehicle_gps_position_s gps;
 	memset(&gps, 0, sizeof(gps));
-	orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps);
+	orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
 
-	struct gps_module_msg msg = { 0 };
+	struct gps_module_msg msg;
 	*size = sizeof(msg);
 	memset(&msg, 0, *size);
 
@@ -150,7 +236,7 @@ build_gps_response(uint8_t *buffer, size_t *size)
 		msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F);
 
 		/* GPS speed */
-		uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6);
+		uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f);
 		msg.gps_speed_L = (uint8_t)speed & 0xff;
 		msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff;
 		
@@ -196,33 +282,33 @@ build_gps_response(uint8_t *buffer, size_t *size)
 		msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff;
 		
 		/* Altitude */
-		uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f);
+		uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f);
 		msg.altitude_L = (uint8_t)alt & 0xff;
 		msg.altitude_H = (uint8_t)(alt >> 8) & 0xff;
 
-		/* Get any (and probably only ever one) home_sub postion report */
+		/* Get any (and probably only ever one) _home_sub postion report */
 		bool updated;
-		orb_check(home_sub, &updated);
+		orb_check(_home_sub, &updated);
 		if (updated) {
 			/* get a local copy of the home position data */
 			struct home_position_s home;
 			memset(&home, 0, sizeof(home));
-			orb_copy(ORB_ID(home_position), home_sub, &home);
+			orb_copy(ORB_ID(home_position), _home_sub, &home);
 
-			home_lat = ((double)(home.lat))*1e-7d;
-			home_lon = ((double)(home.lon))*1e-7d;
-			home_position_set = true;
+			_home_lat = ((double)(home.lat))*1e-7d;
+			_home_lon = ((double)(home.lon))*1e-7d;
+			_home_position_set = true;
 		}
 
 		/* Distance from home */
-		if (home_position_set) {
-			uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon);
+		if (_home_position_set) {
+			uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon);
 
 			msg.distance_L = (uint8_t)dist & 0xff;
 			msg.distance_H = (uint8_t)(dist >> 8) & 0xff;
 
 			/* Direction back to home */
-			uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F);
+			uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F);
 			msg.home_direction = (uint8_t)bearing >> 1;
 		}
 	}
diff --git a/src/drivers/hott_telemetry/messages.h b/src/drivers/hott/messages.h
index e6d5cc723..451bee91c 100644
--- a/src/drivers/hott_telemetry/messages.h
+++ b/src/drivers/hott/messages.h
@@ -60,19 +60,25 @@
 #define STOP_BYTE		0x7d
 #define TEMP_ZERO_CELSIUS	0x14
 
+/* The GAM Module poll message. */
+struct gam_module_poll_msg {
+	uint8_t mode;
+	uint8_t id;
+};
+
 /* Electric Air Module (EAM) constants. */
 #define EAM_SENSOR_ID			0x8e
 #define EAM_SENSOR_TEXT_ID		0xe0
 
 /* The Electric Air Module message. */
 struct eam_module_msg {
-	uint8_t start;				/**< Start byte   				*/
+	uint8_t start;			/**< Start byte   				*/
 	uint8_t eam_sensor_id;		/**< EAM sensor					*/
 	uint8_t warning;
-	uint8_t sensor_id;			/**< Sensor ID, why different?	*/
+	uint8_t sensor_text_id;
 	uint8_t alarm_inverse1;
 	uint8_t alarm_inverse2;
-	uint8_t cell1_L;			/**< Lipo cell voltages. Not supported.	*/
+	uint8_t cell1_L;		/**< Lipo cell voltages. Not supported.	*/
 	uint8_t cell2_L;
 	uint8_t cell3_L;
 	uint8_t cell4_L;
@@ -92,9 +98,9 @@ struct eam_module_msg {
 	uint8_t batt2_voltage_H;
 	uint8_t temperature1;		/**< Temperature sensor 1. 20 = 0 degrees */
 	uint8_t temperature2;
-	uint8_t altitude_L;			/**< Attitude (meters) lower 8-bits. 500 = 0 meters */
+	uint8_t altitude_L;		/**< Attitude (meters) lower 8-bits. 500 = 0 meters */
 	uint8_t altitude_H;
-	uint8_t current_L;			/**< Current (mAh) lower 8-bits in steps of 0.1V */
+	uint8_t current_L;		/**< Current (mAh) lower 8-bits in steps of 0.1V */
 	uint8_t current_H;
 	uint8_t main_voltage_L;		/**< Main power voltage lower 8-bits in steps of 0.1V */
 	uint8_t main_voltage_H;
@@ -103,35 +109,82 @@ struct eam_module_msg {
 	uint8_t climbrate_L;		/**< Climb rate in 0.01m/s. 0m/s = 30000 */
 	uint8_t climbrate_H;
 	uint8_t climbrate_3s;		/**< Climb rate in m/3sec. 0m/3sec = 120 */
-	uint8_t rpm_L;				/**< RPM Lower 8-bits In steps of 10 U/min */
+	uint8_t rpm_L;			/**< RPM Lower 8-bits In steps of 10 U/min */
 	uint8_t rpm_H;
 	uint8_t electric_min;		/**< Flight time in minutes. */
 	uint8_t electric_sec;		/**< Flight time in seconds. */
-	uint8_t speed_L;			/**< Airspeed in km/h in steps of 1 km/h */
+	uint8_t speed_L;		/**< Airspeed in km/h in steps of 1 km/h */
 	uint8_t speed_H;
-	uint8_t stop;				/**< Stop byte */
-	uint8_t checksum;			/**< Lower 8-bits of all bytes summed. */
+	uint8_t stop;			/**< Stop byte */
+	uint8_t checksum;		/**< Lower 8-bits of all bytes summed. */
 };
 
-/** 
- * The maximum buffer size required to store a HoTT message.
- */
-#define MESSAGE_BUFFER_SIZE sizeof(union { 	\
-    struct eam_module_msg eam; 				\
-})
+
+/* General Air Module (GAM) constants. */
+#define GAM_SENSOR_ID			0x8d
+#define GAM_SENSOR_TEXT_ID		0xd0
+
+struct gam_module_msg {
+        uint8_t start;		// start byte constant value 0x7c
+        uint8_t gam_sensor_id;		// EAM sensort id. constat value 0x8d
+        uint8_t warning_beeps;		// 1=A 2=B ... 0x1a=Z  0 = no alarm
+        uint8_t sensor_text_id;		// constant value 0xd0
+        uint8_t alarm_invers1;		// alarm bitmask. Value is displayed inverted
+        uint8_t alarm_invers2;		// alarm bitmask. Value is displayed inverted                                                                
+        uint8_t cell1;			// cell 1 voltage lower value. 0.02V steps, 124=2.48V
+        uint8_t cell2;
+        uint8_t cell3;
+        uint8_t cell4;
+        uint8_t cell5;
+        uint8_t cell6;
+        uint8_t batt1_L;		// battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V
+        uint8_t batt1_H;
+        uint8_t batt2_L;		// battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V
+        uint8_t batt2_H;
+        uint8_t temperature1;		// temperature 1. offset of 20. a value of 20 = 0°C
+        uint8_t temperature2;		// temperature 2. offset of 20. a value of 20 = 0°C
+        uint8_t fuel_procent;		// Fuel capacity in %. Values 0--100
+					// graphical display ranges: 0-25% 50% 75% 100%
+        uint8_t fuel_ml_L;		// Fuel in ml scale. Full = 65535!
+        uint8_t fuel_ml_H;		//
+        uint8_t rpm_L;			// RPM in 10 RPM steps. 300 = 3000rpm
+        uint8_t rpm_H;			//
+        uint8_t altitude_L;		// altitude in meters. offset of 500, 500 = 0m
+        uint8_t altitude_H;		//
+        uint8_t climbrate_L;		// climb rate in 0.01m/s. Value of 30000 = 0.00 m/s
+        uint8_t climbrate_H;		//
+        uint8_t climbrate3s;		// climb rate in m/3sec. Value of 120 = 0m/3sec
+        uint8_t current_L;		// current in 0.1A steps
+        uint8_t current_H;		//
+        uint8_t main_voltage_L;		// Main power voltage using 0.1V steps
+        uint8_t main_voltage_H;		//
+        uint8_t batt_cap_L;		// used battery capacity in 10mAh steps
+        uint8_t batt_cap_H;		//
+        uint8_t speed_L;		// (air?) speed in km/h(?) we are using ground speed here per default
+        uint8_t speed_H;		//
+        uint8_t min_cell_volt;		// minimum cell voltage in 2mV steps. 124 = 2,48V
+        uint8_t min_cell_volt_num;	// number of the cell with the lowest voltage
+        uint8_t rpm2_L;			// RPM in 10 RPM steps. 300 = 3000rpm
+        uint8_t rpm2_H;			//
+        uint8_t general_error_number;	// Voice error == 12. TODO: more docu
+        uint8_t pressure;		// Pressure up to 16bar. 0,1bar scale. 20 = 2bar
+        uint8_t version;		// version number TODO: more info?
+        uint8_t stop;			// stop byte
+        uint8_t checksum;		// checksum
+};
 
 /* GPS sensor constants. */
-#define GPS_SENSOR_ID		0x8A
-#define GPS_SENSOR_TEXT_ID	0xA0
+#define GPS_SENSOR_ID		0x8a
+#define GPS_SENSOR_TEXT_ID	0xa0
 
 /** 
  * The GPS sensor message
  * Struct based on: https://code.google.com/p/diy-hott-gps/downloads 
  */
 struct gps_module_msg { 
-	uint8_t start;				/**< Start byte */
-	uint8_t sensor_id;			/**< GPS sensor ID*/
-	uint8_t warning;			/**< Byte 3: 0…= warning beeps */
+	uint8_t start;			/**< Start byte */
+	uint8_t sensor_id;		/**< GPS sensor ID*/
+	uint8_t warning;		/**< Byte 3: 0…= warning beeps */
 	uint8_t sensor_text_id;		/**< GPS Sensor text mode ID */
 	uint8_t alarm_inverse1;		/**< Byte 5: 01 inverse status */
 	uint8_t alarm_inverse2;		/**< Byte 6: 00 inverse status status 1 = no GPS Signal */
@@ -179,15 +232,15 @@ struct gps_module_msg {
 	uint8_t checksum;			/**< Byte 45: Parity Byte */
 };
 
-/** 
- * The maximum buffer size required to store a HoTT message.
- */
-#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { 	\
-    struct gps_module_msg gps; 				\
-})
+// The maximum size of a message.
+#define MAX_MESSAGE_BUFFER_SIZE 45
 
-void messages_init(void);
+void init_sub_messages(void);
+void init_pub_messages(void);
+void build_gam_request(uint8_t *buffer, size_t *size);
+void publish_gam_message(const uint8_t *buffer);
 void build_eam_response(uint8_t *buffer, size_t *size);
+void build_gam_response(uint8_t *buffer, size_t *size);
 void build_gps_response(uint8_t *buffer, size_t *size);
 float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next);
 void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec);
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 7928752d5..70147d56a 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -195,7 +195,7 @@ PX4FMU	*g_fmu;
 } // namespace
 
 PX4FMU::PX4FMU() :
-	CDev("fmuservo", "/dev/px4fmu"),
+	CDev("fmuservo", PX4FMU_DEVICE_PATH),
 	_mode(MODE_NONE),
 	_pwm_default_rate(50),
 	_pwm_alt_rate(50),
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 9f84c0950..e9f26b0d3 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -132,6 +132,16 @@ public:
 	 */
 	void			print_status();
 
+	inline void		set_dsm_vcc_ctl(bool enable)
+	{
+		_dsm_vcc_ctl = enable;
+	};
+
+	inline bool		get_dsm_vcc_ctl()
+	{
+		return _dsm_vcc_ctl;
+	};
+
 private:
 	device::Device		*_interface;
 
@@ -179,6 +189,12 @@ private:
 	uint64_t		_battery_last_timestamp;
 
 	/**
+	 * Relay1 is dedicated to controlling DSM receiver power
+	 */
+
+	bool			_dsm_vcc_ctl;
+
+	/**
 	 * Trampoline to the worker task
 	 */
 	static void		task_main_trampoline(int argc, char *argv[]);
@@ -319,7 +335,7 @@ PX4IO	*g_dev;
 }
 
 PX4IO::PX4IO(device::Device *interface) :
-	CDev("px4io", "/dev/px4io"),
+	CDev("px4io", PX4IO_DEVICE_PATH),
 	_interface(interface),
 	_hardware(0),
 	_max_actuators(0),
@@ -346,7 +362,8 @@ PX4IO::PX4IO(device::Device *interface) :
 	_battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor
 	_battery_amp_bias(0),
 	_battery_mamphour_total(0),
-	_battery_last_timestamp(0)
+	_battery_last_timestamp(0),
+	_dsm_vcc_ctl(false)
 {
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
@@ -1264,13 +1281,14 @@ PX4IO::print_status()
 	printf("%u bytes free\n",
 		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
-	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
+	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
 		flags,
 		((flags & PX4IO_P_STATUS_FLAGS_ARMED)    ? " ARMED" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_RC_DSM)   ? " DSM" : ""),
+		(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
+		(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PPM" : ""),
@@ -1373,7 +1391,8 @@ PX4IO::print_status()
 }
 
 int
-PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
+PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
+/* Make it obvious that file * isn't used here */
 {
 	int ret = OK;
 
@@ -1425,6 +1444,26 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		*(unsigned *)arg = _max_actuators;
 		break;
 
+	case DSM_BIND_START:
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); 
+		usleep(500000);
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
+		usleep(1000);
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
+		usleep(100000);
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
+		break;
+
+	case DSM_BIND_STOP:
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
+		usleep(500000);
+		break;
+
+	case DSM_BIND_POWER_UP:
+		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
+		break;
+
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): {
 
 		/* TODO: we could go lower for e.g. TurboPWM */
@@ -1467,18 +1506,31 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg)
 		break;
 	}
 
-	case GPIO_RESET:
-		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0);
+	case GPIO_RESET: {
+		uint32_t bits = (1 << _max_relays) - 1;
+		/* don't touch relay1 if it's controlling RX vcc */
+		if (_dsm_vcc_ctl)
+			bits &= ~PX4IO_RELAY1;
+		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0);
 		break;
+	}
 
 	case GPIO_SET:
 		arg &= ((1 << _max_relays) - 1);
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
+		/* don't touch relay1 if it's controlling RX vcc */
+		if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
+			ret = -EINVAL;
+		else
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg);
 		break;
 
 	case GPIO_CLEAR:
 		arg &= ((1 << _max_relays) - 1);
-		ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
+		/* don't touch relay1 if it's controlling RX vcc */
+		if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1))
+			ret = -EINVAL;
+		else
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0);
 		break;
 
 	case GPIO_GET:
@@ -1646,10 +1698,65 @@ start(int argc, char *argv[])
 		errx(1, "driver init failed");
 	}
 
+	int dsm_vcc_ctl;
+
+	if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) {
+		if (dsm_vcc_ctl) {
+			g_dev->set_dsm_vcc_ctl(true);
+			g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
+		}
+	}
 	exit(0);
 }
 
 void
+bind(int argc, char *argv[])
+{
+	int pulses;
+
+	if (g_dev == nullptr)
+		errx(1, "px4io must be started first");
+
+	if (!g_dev->get_dsm_vcc_ctl())
+		errx(1, "DSM bind feature not enabled");
+
+	if (argc < 3)
+		errx(0, "needs argument, use dsm2 or dsmx");
+
+	if (!strcmp(argv[2], "dsm2"))
+		pulses = 3;
+	else if (!strcmp(argv[2], "dsmx"))
+		pulses = 7;
+	else 
+		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
+
+	/* Open console directly to grab CTRL-C signal */
+	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
+	if (!console)
+		errx(1, "failed opening console");
+
+	warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1.");
+	warnx("Press CTRL-C or 'c' when done.");
+
+	g_dev->ioctl(nullptr, DSM_BIND_START, pulses);
+
+	for (;;) {
+		usleep(500000L);
+		/* Check if user wants to quit */
+		char c;
+		if (read(console, &c, 1) == 1) {
+			if (c == 0x03 || c == 0x63) {
+				warnx("Done\n");
+				g_dev->ioctl(nullptr, DSM_BIND_STOP, 0);
+				g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0);
+				close(console);
+	            exit(0);
+            }
+		}
+	}
+}
+
+void
 test(void)
 {
 	int		fd;
@@ -1658,7 +1765,7 @@ test(void)
 	int		direction = 1;
 	int		ret;
 
-	fd = open("/dev/px4io", O_WRONLY);
+	fd = open(PX4IO_DEVICE_PATH, O_WRONLY);
 
 	if (fd < 0)
 		err(1, "failed to open device");
@@ -1895,7 +2002,6 @@ px4io_main(int argc, char *argv[])
 	 	 * doesn't reference filp in ioctl()
 		 */
 		g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
-
 		exit(0);
 	}
 
@@ -1928,7 +2034,7 @@ px4io_main(int argc, char *argv[])
 		/* we can cheat and call the driver directly, as it
 		 * doesn't reference filp in ioctl()
 		 */
-		int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level);
+		int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level);
 		if (ret != 0) {
 			printf("SET_DEBUG failed - %d\n", ret);
 			exit(1);
@@ -1950,6 +2056,9 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "monitor"))
 		monitor();
 
-out:
-	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'");
+	if (!strcmp(argv[1], "bind"))
+		bind(argc, argv);
+
+	out:
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'");
 }
diff --git a/src/drivers/px4io/px4io_i2c.cpp b/src/drivers/px4io/px4io_i2c.cpp
index f561b4359..e0c3e5608 100644
--- a/src/drivers/px4io/px4io_i2c.cpp
+++ b/src/drivers/px4io/px4io_i2c.cpp
@@ -78,7 +78,7 @@ device::Device
 }
 
 PX4IO_I2C::PX4IO_I2C(int bus, uint8_t address) :
-	I2C("PX4IO_i2c", nullptr, bus, address, 400000)
+	I2C("PX4IO_i2c", nullptr, bus, address, 320000)
 {
 	_retries = 3;
 }
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 97d7fdd75..191d20f30 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -661,10 +661,10 @@ int KalmanNav::correctPos()
 	Vector y(6);
 	y(0) = _gps.vel_n_m_s - vN;
 	y(1) = _gps.vel_e_m_s - vE;
-	y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG;
-	y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG;
-	y(4) = double(_gps.alt) / 1.0e3 - alt;
-	y(5) = double(_sensors.baro_alt_meter) - alt;
+	y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG;
+	y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG;
+	y(4) = _gps.alt / 1.0e3f - alt;
+	y(5) = _sensors.baro_alt_meter - alt;
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -698,7 +698,7 @@ int KalmanNav::correctPos()
 	vD += xCorrect(VD);
 	lat += double(xCorrect(LAT));
 	lon += double(xCorrect(LON));
-	alt += double(xCorrect(ALT));
+	alt += xCorrect(ALT);
 
 	// update state covariance
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
@@ -710,7 +710,7 @@ int KalmanNav::correctPos()
 	static int counter = 0;
 	if (beta > _faultPos.get() && (counter % 10 == 0)) {
 		warnx("fault in gps: beta = %8.4f", (double)beta);
-		warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
+		warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f",
 		       double(y(0) / sqrtf(RPos(0, 0))),
 		       double(y(1) / sqrtf(RPos(1, 1))),
 		       double(y(2) / sqrtf(RPos(2, 2))),
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c
index 67f053e22..928d9b85e 100644
--- a/src/modules/commander/commander.c
+++ b/src/modules/commander/commander.c
@@ -1389,6 +1389,7 @@ int commander_thread_main(int argc, char *argv[])
 	uint64_t start_time = hrt_absolute_time();
 	uint64_t failsave_ll_start_time = 0;
 
+	enum VEHICLE_MANUAL_SAS_MODE	manual_sas_mode;
 	bool state_changed = true;
 	bool param_init_forced = true;
 
@@ -1828,8 +1829,9 @@ int commander_thread_main(int argc, char *argv[])
 
 				} else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) {
 
-					/* bottom stick position, set altitude hold */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
+					/* bottom stick position, set default */
+					/* this MUST be mapped to extremal position to switch easy in case of emergency */
+					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
 
 				} else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) {
 
@@ -1837,8 +1839,14 @@ int commander_thread_main(int argc, char *argv[])
 					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE;
 
 				} else {
-					/* center stick position, set default */
-					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS;
+					/* center stick position, set altitude hold */
+					current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE;
+				}
+
+				if (current_status.manual_sas_mode != manual_sas_mode) {
+					/* publish SAS mode changes immediately */
+					manual_sas_mode = current_status.manual_sas_mode;
+					state_changed = true;
 				}
 
 				/*
@@ -1849,8 +1857,10 @@ int commander_thread_main(int argc, char *argv[])
 				     (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
 				     (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
 				    ) &&
-				    ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) &&
-				    (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
+					current_status.flag_control_manual_enabled &&
+					current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
+				    sp_man.yaw < -STICK_ON_OFF_LIMIT &&
+				    sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 						update_state_machine_disarm(stat_pub, &current_status, mavlink_fd);
 						stick_on_counter = 0;
@@ -1862,7 +1872,10 @@ int commander_thread_main(int argc, char *argv[])
 				}
 
 				/* check if left stick is in lower right position --> arm */
-				if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) {
+				if (current_status.flag_control_manual_enabled &&
+					current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS &&
+					sp_man.yaw > STICK_ON_OFF_LIMIT &&
+					sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 					if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 						update_state_machine_arm(stat_pub, &current_status, mavlink_fd);
 						stick_on_counter = 0;
diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 8b4c0cb30..1aef739c7 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -53,11 +53,7 @@
 #include <uORB/topics/vehicle_status.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
-
-#define PX4IO_RELAY1	(1<<0)
-#define PX4IO_RELAY2	(1<<1)
-#define PX4IO_ACC1		(1<<2)
-#define PX4IO_ACC2		(1<<3)
+#include <modules/px4iofirmware/protocol.h>
 
 struct gpio_led_s {
 	struct work_s work;
@@ -186,10 +182,9 @@ void gpio_led_start(FAR void *arg)
 	char *gpio_dev;
 
 	if (priv->use_io) {
-		gpio_dev = "/dev/px4io";
-
+		gpio_dev = PX4IO_DEVICE_PATH;
 	} else {
-		gpio_dev = "/dev/px4fmu";
+		gpio_dev = PX4FMU_DEVICE_PATH;
 	}
 
 	/* open GPIO device */
@@ -203,6 +198,7 @@ void gpio_led_start(FAR void *arg)
 	}
 
 	/* configure GPIO pin */
+	/* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */
 	ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin);
 
 	/* subscribe to vehicle status topic */
@@ -263,7 +259,6 @@ void gpio_led_cycle(FAR void *arg)
 
 		if (led_state_new) {
 			ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
-
 		} else {
 			ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
 		}
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index 405046750..eea928a17 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -373,7 +373,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt);
 
 		} else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
-			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt);
+			dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt);
 
 		} else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) {
 			dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z);
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 5a95a8aa9..74037d1c2 100644
--- a/src/modules/px4iofirmware/controls.c
+++ b/src/modules/px4iofirmware/controls.c
@@ -95,9 +95,16 @@ controls_tick() {
 	 */
 
 	perf_begin(c_gather_dsm);
-	bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count);
-	if (dsm_updated)
+	uint16_t temp_count = r_raw_rc_count;
+	bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count);
+	if (dsm_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
+		r_raw_rc_count = temp_count & 0x7fff;
+		if (temp_count & 0x8000)
+			r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11;
+		else
+			r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11;
+	}
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c
index ea35e5513..ab6e3fec4 100644
--- a/src/modules/px4iofirmware/dsm.c
+++ b/src/modules/px4iofirmware/dsm.c
@@ -40,6 +40,7 @@
  */
 
 #include <nuttx/config.h>
+#include <nuttx/arch.h>
 
 #include <fcntl.h>
 #include <unistd.h>
@@ -101,6 +102,41 @@ dsm_init(const char *device)
 	return dsm_fd;
 }
 
+void
+dsm_bind(uint16_t cmd, int pulses)
+{
+	const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10;
+
+	if (dsm_fd < 0)
+		return;
+
+	switch (cmd) {
+	case dsm_bind_power_down:
+		// power down DSM satellite
+		POWER_RELAY1(0);
+		break;
+	case dsm_bind_power_up:
+		POWER_RELAY1(1);
+		dsm_guess_format(true);
+		break;
+	case dsm_bind_set_rx_out:
+		stm32_configgpio(usart1RxAsOutp); 
+		break;
+	case dsm_bind_send_pulses:
+		for (int i = 0; i < pulses; i++) {
+			stm32_gpiowrite(usart1RxAsOutp, false);
+			up_udelay(50);
+			stm32_gpiowrite(usart1RxAsOutp, true);
+			up_udelay(50);
+		}
+		break;
+	case dsm_bind_reinit_uart:
+		// Restore USART rx pin
+		stm32_configgpio(GPIO_USART1_RX);
+		break;
+	}
+}
+
 bool
 dsm_input(uint16_t *values, uint16_t *num_values)
 {
@@ -218,7 +254,7 @@ dsm_guess_format(bool reset)
 
 	/*
 	 * Iterate the set of sensible sniffed channel sets and see whether
-	 * decoding in 10 or 11-bit mode has yielded anything we recognise.
+	 * decoding in 10 or 11-bit mode has yielded anything we recognize.
 	 *
 	 * XXX Note that due to what seem to be bugs in the DSM2 high-resolution
 	 *     stream, we may want to sniff for longer in some cases when we think we
@@ -349,6 +385,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		values[channel] = value;
 	}
 
+	if (channel_shift == 11)
+		*num_values |= 0x8000;
+
 	/*
 	 * XXX Note that we may be in failsafe here; we need to work out how to detect that.
 	 */
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index fa57dfc3f..dc5c40638 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -106,6 +106,7 @@
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 12) /* DSM input is 11 bit data */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* [1] VBatt is very close to regulator dropout */
@@ -169,7 +170,14 @@
 
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* [1] battery voltage correction factor (float) */
 #define PX4IO_P_SETUP_VSERVO_SCALE		6	/* [2] servo voltage correction factor (float) */
- 					     /*	7 */
+#define PX4IO_P_SETUP_DSM			7	/* DSM bind state */
+enum {							/* DSM bind states */
+	dsm_bind_power_down = 0,
+	dsm_bind_power_up,
+	dsm_bind_set_rx_out,
+	dsm_bind_send_pulses,
+	dsm_bind_reinit_uart
+};
  					     /*	8 */
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index ccf175e45..bd78f2638 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -200,6 +200,7 @@ extern void	controls_init(void);
 extern void	controls_tick(void);
 extern int	dsm_init(const char *device);
 extern bool	dsm_input(uint16_t *values, uint16_t *num_values);
+extern void     dsm_bind(uint16_t cmd, int pulses);
 extern int	sbus_init(const char *device);
 extern bool	sbus_input(uint16_t *values, uint16_t *num_values);
 
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 873ee73f1..9606faa86 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -360,16 +360,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
 			r_setup_relays = value;
 #ifdef POWER_RELAY1
-			POWER_RELAY1(value & (1 << 0) ? 1 : 0);
+			POWER_RELAY1((value & PX4IO_P_SETUP_RELAYS_POWER1) ? 1 : 0);
 #endif
 #ifdef POWER_RELAY2
-			POWER_RELAY2(value & (1 << 1) ? 1 : 0);
+			POWER_RELAY2((value & PX4IO_P_SETUP_RELAYS_POWER2) ? 1 : 0);
 #endif
 #ifdef POWER_ACC1
-			POWER_ACC1(value & (1 << 2) ? 1 : 0);
+			POWER_ACC1((value & PX4IO_P_SETUP_RELAYS_ACC1) ? 1 : 0);
 #endif
 #ifdef POWER_ACC2
-			POWER_ACC2(value & (1 << 3) ? 1 : 0);
+			POWER_ACC2((value & PX4IO_P_SETUP_RELAYS_ACC2) ? 1 : 0);
 #endif
 			break;
 
@@ -382,6 +382,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]);
 			break;
 
+		case PX4IO_P_SETUP_DSM:
+			dsm_bind(value & 0x0f, (value >> 4) & 7);
+			break;
+
 		default:
 			return -1;
 		}
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 3e6b20472..3713e0b30 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -72,6 +72,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_global_position_setpoint.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/optical_flow.h>
@@ -615,7 +616,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
 	/* number of messages */
-	const ssize_t fdsc = 18;
+	const ssize_t fdsc = 19;
 	/* Sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
@@ -637,6 +638,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct vehicle_local_position_s local_pos;
 		struct vehicle_local_position_setpoint_s local_pos_sp;
 		struct vehicle_global_position_s global_pos;
+		struct vehicle_global_position_setpoint_s global_pos_sp;
 		struct vehicle_gps_position_s gps_pos;
 		struct vehicle_vicon_position_s vicon_pos;
 		struct optical_flow_s flow;
@@ -660,6 +662,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int local_pos_sub;
 		int local_pos_sp_sub;
 		int global_pos_sub;
+		int global_pos_sp_sub;
 		int gps_pos_sub;
 		int vicon_pos_sub;
 		int flow_sub;
@@ -689,6 +692,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_ARSP_s log_ARSP;
 			struct log_FLOW_s log_FLOW;
 			struct log_GPOS_s log_GPOS;
+			struct log_GPSP_s log_GPSP;
 			struct log_ESC_s log_ESC;
 		} body;
 	} log_msg = {
@@ -775,6 +779,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;
 
+	/* --- GLOBAL POSITION SETPOINT--- */
+	subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint));
+	fds[fdsc_count].fd = subs.global_pos_sp_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* --- VICON POSITION --- */
 	subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position));
 	fds[fdsc_count].fd = subs.vicon_pos_sub;
@@ -1077,6 +1087,25 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(GPOS);
 			}
 
+			/* --- GLOBAL POSITION SETPOINT --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp);
+				log_msg.msg_type = LOG_GPSP_MSG;
+				log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative;
+				log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat;
+				log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon;
+				log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude;
+				log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw;
+				log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius;
+				log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction;
+				log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd;
+				log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1;
+				log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2;
+				log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3;
+				log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4;
+				LOGBUFFER_WRITE_AND_COUNT(GPSP);
+			}
+
 			/* --- VICON POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos);
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index abc882d23..934e4dec8 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -149,15 +149,15 @@ struct log_ATTC_s {
 /* --- STAT - VEHICLE STATE --- */
 #define LOG_STAT_MSG 10
 struct log_STAT_s {
-	unsigned char state;
-	unsigned char flight_mode;
-	unsigned char manual_control_mode;
-	unsigned char manual_sas_mode;
-	unsigned char armed;
+	uint8_t state;
+	uint8_t flight_mode;
+	uint8_t manual_control_mode;
+	uint8_t manual_sas_mode;
+	uint8_t armed;
 	float battery_voltage;
 	float battery_current;
 	float battery_remaining;
-	unsigned char battery_warning;
+	uint8_t battery_warning;
 };
 
 /* --- RC - RC INPUT CHANNELS --- */
@@ -210,13 +210,29 @@ struct log_GPOS_s {
 	float vel_d;
 };
 
+/* --- GPSP - GLOBAL POSITION SETPOINT --- */
+#define LOG_GPSP_MSG 17
+struct log_GPSP_s {
+	uint8_t altitude_is_relative;
+	int32_t lat;
+	int32_t lon;
+	float altitude;
+	float yaw;
+	float loiter_radius;
+	int8_t loiter_direction;
+	uint8_t nav_cmd;
+	float param1;
+	float param2;
+	float param3;
+	float param4;
+};
+
 /* --- ESC - ESC STATE --- */
-#define LOG_ESC_MSG 64
+#define LOG_ESC_MSG 18
 struct log_ESC_s {
 	uint16_t counter;
 	uint8_t esc_count;
 	uint8_t esc_connectiontype;
-
 	uint8_t esc_num;
 	uint16_t esc_address;
 	uint16_t esc_version;
@@ -227,6 +243,7 @@ struct log_ESC_s {
 	float    esc_setpoint;
 	uint16_t esc_setpoint_raw;
 };
+
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -248,6 +265,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
+	LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"),
 	LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"),
 };
 
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index f6f4d60c7..252c1b7a9 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -155,6 +155,7 @@ PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
 
 PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
+PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 1ded14a91..ae5a55109 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -230,6 +230,8 @@ private:
 		float rc_scale_flaps;
 
 		float battery_voltage_scaling;
+
+		int   rc_rl1_DSM_VCC_control;
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -278,6 +280,8 @@ private:
 		param_t rc_scale_flaps;
 
 		param_t battery_voltage_scaling;
+
+		param_t rc_rl1_DSM_VCC_control;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -514,6 +518,9 @@ Sensors::Sensors() :
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
+	/* DSM VCC relay control */
+	_parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -730,6 +737,11 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}
 
+	/* relay 1 DSM VCC control */
+	if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) {
+		warnx("Failed updating relay 1 DSM VCC control");
+	}
+
 	return OK;
 }
 
diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
index e67a39e1e..00cf59b28 100644
--- a/src/modules/uORB/topics/esc_status.h
+++ b/src/modules/uORB/topics/esc_status.h
@@ -63,7 +63,8 @@
 
 enum ESC_VENDOR {
 	ESC_VENDOR_GENERIC = 0,					/**< generic ESC */
-	ESC_VENDOR_MIKROKOPTER					/**< Mikrokopter */
+	ESC_VENDOR_MIKROKOPTER,					/**< Mikrokopter */
+	ESC_VENDOR_GRAUPNER_HOTT				/**< Graupner HoTT ESC */	
 };
 
 enum ESC_CONNECTION_TYPE {
diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
index 3ae3ff28c..5c8ce1e4d 100644
--- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
@@ -66,7 +66,7 @@ struct vehicle_global_position_setpoint_s
 	float altitude;			/**< altitude in meters					*/
 	float yaw;			/**< in radians NED -PI..+PI 				*/
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
-	uint8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
+	int8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
 	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
 	float param1;
 	float param2;
diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c
index ab536d956..62a873270 100644
--- a/src/systemcmds/tests/test_gpio.c
+++ b/src/systemcmds/tests/test_gpio.c
@@ -92,7 +92,7 @@ int test_gpio(int argc, char *argv[])
 	int		fd;
 	int		ret = 0;
 
-	fd = open(GPIO_DEVICE_PATH, 0);
+	fd = open(PX4IO_DEVICE_PATH, 0);
 
 	if (fd < 0) {
 		printf("GPIO: open fail\n");