Merged master

56 files changed, 1908 insertions, 940 deletions

diff --git a/Documentation/dsm_bind.odt b/Documentation/dsm_bind.odt
new file mode 100644
index 000000000..66ea1f1be
--- /dev/null
+++ b/Documentation/dsm_bind.odt
diff --git a/Documentation/dsm_bind.pdf b/Documentation/dsm_bind.pdf
new file mode 100644
index 000000000..e62d1ed83
--- /dev/null
+++ b/Documentation/dsm_bind.pdf
diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk
index ea1e38ff7..fc87bedd5 100644
--- a/makefiles/config_px4fmu-v1_default.mk
+++ b/makefiles/config_px4fmu-v1_default.mk
@@ -27,7 +27,8 @@ MODULES		+= drivers/ms5611
 MODULES		+= drivers/mb12xx
 MODULES		+= drivers/gps
 MODULES		+= drivers/hil
-MODULES		+= drivers/hott_telemetry
+MODULES		+= drivers/hott/hott_telemetry
+MODULES		+= drivers/hott/hott_sensors
 MODULES		+= drivers/blinkm
 MODULES		+= drivers/mkblctrl
 MODULES		+= drivers/md25
diff --git a/mavlink/include/mavlink/v1.0/mavlink_conversions.h b/mavlink/include/mavlink/v1.0/mavlink_conversions.h
index 1dc9088b7..d89363577 100644
--- a/mavlink/include/mavlink/v1.0/mavlink_conversions.h
+++ b/mavlink/include/mavlink/v1.0/mavlink_conversions.h
@@ -1,6 +1,12 @@
 #ifndef  _MAVLINK_CONVERSIONS_H_
 #define  _MAVLINK_CONVERSIONS_H_
 
+/* enable math defines on Windows */
+#ifdef _MSC_VER
+#ifndef _USE_MATH_DEFINES
+#define _USE_MATH_DEFINES
+#endif
+#endif
 #include <math.h>
 
 /**
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 7e3998fca..510983d4b 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -63,43 +63,12 @@
  * 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
-/*
- * PX4IO GPIO numbers.
- *
- * XXX note that these are here for reference/future use; currently
- * there is no good way to wire these up without a common STM32 GPIO
- * driver, which isn't implemented yet.
- */
-/* outputs */
-# define GPIO_ACC1_POWER	(1<<0)		/**< accessory power 1 */
-# define GPIO_ACC2_POWER	(1<<1)		/**< accessory power 2 */
-# define GPIO_SERVO_POWER	(1<<2)		/**< servo power */
-# define GPIO_RELAY1		(1<<3)		/**< relay 1 */
-# define GPIO_RELAY2		(1<<4)		/**< relay 2 */
-# define GPIO_LED_BLUE		(1<<5)		/**< blue LED */
-# define GPIO_LED_AMBER		(1<<6)		/**< amber/red LED */
-# define GPIO_LED_SAFETY	(1<<7)		/**< safety LED */
-
-/* inputs */
-# define GPIO_ACC_OVERCURRENT	(1<<8)		/**< accessory 1/2 overcurrent detect */
-# define GPIO_SERVO_OVERCURRENT	(1<<9)		/**< servo overcurrent detect */
-# define GPIO_SAFETY_BUTTON	(1<<10)		/**< safety button pressed */
-
-/**
- * Default GPIO device - other devices may also support this protocol if
- * they also export GPIO-like things.  This is always the GPIOs on the
- * main board.
- */
-# define GPIO_DEVICE_PATH	"/dev/px4io"
-
-#endif
-
-#ifndef GPIO_DEVICE_PATH
+#ifndef PX4IO_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 f2e7ca67d..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));
+
+	_message_class_needed = UBX_CLASS_CFG;
+	_message_id_needed = UBX_MESSAGE_CFG_NAV5;
 
-		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,
-			1);
-		// /* 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,
-			1);
-		// /* 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,
-			1);
-		// /* 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,
-			0);
-		// /* insist of receiving the ACK for this packet */
-		// if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0)
-		// 	continue;
-
-		_waiting_for_ack = false;
-		return 0;
+	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;
+	}
+
+	/* 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;
 	}
-	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
@@ -224,56 +254,56 @@ UBX::receive(unsigned timeout)
 	fds[0].fd = _fd;
 	fds[0].events = POLLIN;
 
-	uint8_t buf[32];
+	uint8_t buf[128];
 
 	/* timeout additional to poll */
 	uint64_t time_started = hrt_absolute_time();
 
-	int j = 0;
 	ssize_t count = 0;
 
-	while (true) {
+	bool handled = false;
 
-		/* pass received bytes to the packet decoder */
-		while (j < count) {
-			if (parse_char(buf[j]) > 0) {
-				/* return to configure during configuration or to the gps driver during normal work
-				 * if a packet has arrived */
-				 if (handle_message() > 0)
-					return 1;
-			}
-			/* in case we keep trying but only get crap from GPS */
-			if (time_started + timeout*1000 < hrt_absolute_time() ) {
-				return -1;
-			}
-			j++;
-		}
-
-		/* everything is read */
-		j = count = 0;
+	while (true) {
 
-		/* then 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));
+				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]) > 0) {
+						/* return to configure during configuration or to the gps driver during normal work
+						 * if a packet has arrived */
+						if (handle_message() > 0)
+							handled = true;
+					}
+				}
 			}
 		}
+
+		/* abort after timeout if no useful packets received */
+		if (time_started + timeout * 1000 < hrt_absolute_time()) {
+			return -1;
+		}
 	}
 }
 
@@ -282,410 +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;
-		/* 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;
-		/* 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_UNINIT:
+		if (b == UBX_SYNC1) {
+			_decode_state = UBX_DECODE_GOT_SYNC1;
+		}
 
-				case UBX_CLASS_NAV:
-					_decode_state = UBX_DECODE_GOT_CLASS;
-					_message_class = NAV;
-					break;
+		break;
 
-//				case UBX_CLASS_RXM:
-//					_decode_state = UBX_DECODE_GOT_CLASS;
-//					_message_class = RXM;
-//					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();
+			/* don't return error, it can be just false sync1 */
+		}
 
-				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:
-			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;
+		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;
-				}
-				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");
-						}
+		/* 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);
+		_message_class = b;
+		_decode_state = UBX_DECODE_GOT_CLASS;
+		break;
 
-						return 1;
-					} else if (_rx_count < RECV_BUFFER_SIZE) {
-						_rx_count++;
-					} else {
-						warnx("ubx: buffer full");
-						decode_init();
-						return -1;
-					}
-				break;
-		default:
-			break;
-	}
-	return 0; //XXX ?
-}
+	case UBX_DECODE_GOT_CLASS:
+		add_byte_to_checksum(b);
+		_message_id = b;
+		_decode_state = UBX_DECODE_GOT_MESSAGEID;
+		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;
 
-int
-UBX::handle_message()
-{
-	int ret = 0;
+	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;
 
-	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;
+	case UBX_DECODE_GOT_LENGTH2:
 
-				_gps_position->lat = packet->lat;
-				_gps_position->lon = packet->lon;
-				_gps_position->alt = packet->height_msl;
+		/* Add to checksum if not yet at checksum byte */
+		if (_rx_count < _payload_size)
+			add_byte_to_checksum(b);
 
-				_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
+		_rx_buffer[_rx_count] = b;
 
-				_rate_count_lat_lon++;
+		/* 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
 
-				/* Add timestamp to finish the report */
-				_gps_position->timestamp_position = hrt_absolute_time();
-				/* only return 1 when new position is available */
-				ret = 1;
+			} else {
+				warnx("ubx: checksum wrong");
+				decode_init();
+				return -1;
 			}
-			break;
+
+		} else if (_rx_count < RECV_BUFFER_SIZE) {
+			_rx_count++;
+
+		} else {
+			warnx("ubx: buffer full");
+			decode_init();
+			return -1;
 		}
 
-		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;
+		break;
 
-				_gps_position->fix_type = packet->gpsFix;
-				_gps_position->s_variance_m_s = packet->sAcc;
-				_gps_position->p_variance_m = packet->pAcc;
+	default:
+		break;
+	}
 
-				_gps_position->timestamp_variance = hrt_absolute_time();
-			}
-			break;
-		}
+	return 0;	// message decoding in progress
+}
 
-//		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");
 
-			if (!_waiting_for_ack) {
-				gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer;
 
-				//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;
+int
+UBX::handle_message()
+{
+	int ret = 0;
 
-				time_t epoch = mktime(&timeinfo);
+	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->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
-				_gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
+					_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->timestamp_time = hrt_absolute_time();
-			}
-			break;
-		}
+					_rate_count_lat_lon++;
 
-		case NAV_SVINFO: {
-//			printf("GOT NAV_SVINFO MESSAGE\n");
+					ret = 1;
+					break;
+				}
 
-			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;
+			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;
 
-				//read checksum
-				const int length_part3 = 2;
-				char _rx_buffer_part3[length_part3];
-				memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3);
+					_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();
 
-				//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
+					ret = 1;
+					break;
+				}
 
-				uint8_t satellites_used = 0;
-				int i;
+			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;
 
-				for (i = 0; i < packet_part1->numCh; i++) { //for each channel
+					/* 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);
+
+					_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();
+
+					ret = 1;
+					break;
+				}
 
-					/* 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;
+			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;
 
+					uint8_t satellites_used = 0;
+					int i;
 
-					/* Write satellite information in the global storage */
-					_gps_position->satellite_prn[i] = packet_part2->svid;
+					//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]);
 
-					//if satellite information is healthy store the data
-					uint8_t unhealthy = packet_part2->flags & 1 << 4; //flags is a bitfield
+						/* write satellite information to global storage */
+						uint8_t sv_used = packet_part2->flags & 0x01;
 
-					if (!unhealthy) {
-						if ((packet_part2->flags) & 1) { //flags is a bitfield
-							_gps_position->satellite_used[i] = 1;
+						if (sv_used) {
+							/* count SVs used for NAV */
 							satellites_used++;
-
-						} else {
-							_gps_position->satellite_used[i] = 0;
 						}
 
+						/* 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);
+					}
 
-					} else {
+					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
 
-				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;
+					if (packet_part1->numCh > 0) {
+						_gps_position->satellite_info_available = true;
+
+					} else {
+						_gps_position->satellite_info_available = false;
+					}
+
+					_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
@@ -695,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
@@ -708,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];
@@ -735,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
@@ -764,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/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index f187f22a6..3fc1054e6 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -75,6 +75,7 @@
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
 #include <uORB/topics/actuator_safety.h>
+#include <uORB/topics/esc_status.h>
 
 #include <systemlib/err.h>
 
@@ -88,7 +89,7 @@
 #define MOTOR_STATE_PRESENT_MASK		0x80
 #define MOTOR_STATE_ERROR_MASK		0x7F
 #define MOTOR_SPINUP_COUNTER			2500
-
+#define ESC_UORB_PUBLISH_DELAY			200
 
 class MK : public device::I2C
 {
@@ -120,6 +121,7 @@ public:
 	int		set_pwm_rate(unsigned rate);
 	int		set_motor_count(unsigned count);
 	int		set_motor_test(bool motortest);
+	int		set_overrideSecurityChecks(bool overrideSecurityChecks);
 	int		set_px4mode(int px4mode);
 	int		set_frametype(int frametype);
 	unsigned int		mk_check_for_blctrl(unsigned int count, bool showOutput);
@@ -137,11 +139,15 @@ private:
 	unsigned int		_motor;
 	int    _px4mode;
 	int    _frametype;
+
 	orb_advert_t	_t_outputs;
 	orb_advert_t	_t_actuators_effective;
+	orb_advert_t	_t_esc_status;
+
 	unsigned int	_num_outputs;
 	bool		_primary_pwm_device;
 	bool     _motortest;
+	bool 		_overrideSecurityChecks;
 
 	volatile bool	_task_should_exit;
 	bool		_armed;
@@ -215,6 +221,7 @@ struct MotorData_t {
 	unsigned int Version;                        // the version of the BL (0 = old)
 	unsigned int SetPoint;                       // written by attitude controller
 	unsigned int SetPointLowerBits;      // for higher Resolution of new BLs
+	float SetPoint_PX4; 			     // Values from PX4
 	unsigned int State;                          // 7 bit for I2C error counter, highest bit indicates if motor is present
 	unsigned int ReadMode;                       // select data to read
 	unsigned short RawPwmValue;							// length of PWM pulse
@@ -244,8 +251,10 @@ MK::MK(int bus) :
 	_t_actuator_safety(-1),
 	_t_outputs(0),
 	_t_actuators_effective(0),
+	_t_esc_status(0),
 	_num_outputs(0),
 	_motortest(false),
+	_overrideSecurityChecks(false),
 	_motor(-1),
 	_px4mode(MAPPING_MK),
 	_frametype(FRAME_PLUS),
@@ -465,6 +474,13 @@ MK::set_motor_test(bool motortest)
 	return OK;
 }
 
+int
+MK::set_overrideSecurityChecks(bool overrideSecurityChecks)
+{
+	_overrideSecurityChecks = overrideSecurityChecks;
+	return OK;
+}
+
 short
 MK::scaling(float val, float inMin, float inMax, float outMin, float outMax)
 {
@@ -507,8 +523,6 @@ MK::task_main()
 	_t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1),
 				   &outputs);
 
-
-
 	/* advertise the effective control inputs */
 	actuator_controls_effective_s controls_effective;
 	memset(&controls_effective, 0, sizeof(controls_effective));
@@ -516,6 +530,12 @@ MK::task_main()
 	_t_actuators_effective = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE : ORB_ID(actuator_controls_effective_1),
 				   &controls_effective);
 
+	/* advertise the blctrl status */
+	esc_status_s esc;
+	memset(&esc, 0, sizeof(esc));
+	_t_esc_status = orb_advertise(ORB_ID(esc_status), &esc);
+
+
 
 	pollfd fds[2];
 	fds[0].fd = _t_actuators;
@@ -603,9 +623,11 @@ MK::task_main()
 						}
 					}
 
-					/* don't go under BLCTRL_MIN_VALUE */
-					if (outputs.output[i] < BLCTRL_MIN_VALUE) {
-						outputs.output[i] = BLCTRL_MIN_VALUE;
+					if(!_overrideSecurityChecks) {
+						/* don't go under BLCTRL_MIN_VALUE */
+						if (outputs.output[i] < BLCTRL_MIN_VALUE) {
+							outputs.output[i] = BLCTRL_MIN_VALUE;
+						}
 					}
 
 					/* output to BLCtrl's */
@@ -613,7 +635,10 @@ MK::task_main()
 						mk_servo_test(i);
 
 					} else {
-						mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024));	// scale the output to 0 - 1024 and sent to output routine
+						//mk_servo_set_value(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 1024));	// scale the output to 0 - 1024 and sent to output routine
+						// 11 Bit
+						Motor[i].SetPoint_PX4 = outputs.output[i];
+						mk_servo_set(i, scaling(outputs.output[i], -1.0f, 1.0f, 0, 2047));	// scale the output to 0 - 2047 and sent to output routine
 					}
 
 				}
@@ -636,8 +661,43 @@ MK::task_main()
 		}
 
 
+
+		/*
+		 * Only update esc topic every half second.
+		 */
+
+		if (hrt_absolute_time() - esc.timestamp > 500000) {
+			esc.counter++;
+			esc.timestamp = hrt_absolute_time();
+			esc.esc_count = (uint8_t) _num_outputs;
+			esc.esc_connectiontype = ESC_CONNECTION_TYPE_I2C;
+
+			for (unsigned int i = 0; i < _num_outputs; i++) {
+				esc.esc[i].esc_address = (uint8_t) BLCTRL_BASE_ADDR + i;
+				esc.esc[i].esc_vendor = ESC_VENDOR_MIKROKOPTER;
+				esc.esc[i].esc_version = (uint16_t) Motor[i].Version;
+				esc.esc[i].esc_voltage = (uint16_t) 0;
+				esc.esc[i].esc_current = (uint16_t) Motor[i].Current;
+				esc.esc[i].esc_rpm = (uint16_t) 0;
+				esc.esc[i].esc_setpoint = (float) Motor[i].SetPoint_PX4;
+				if (Motor[i].Version == 1) {
+					// BLCtrl 2.0 (11Bit)
+					esc.esc[i].esc_setpoint_raw = (uint16_t) (Motor[i].SetPoint<<3) | Motor[i].SetPointLowerBits;
+				} else {
+					// BLCtrl < 2.0 (8Bit)
+					esc.esc[i].esc_setpoint_raw = (uint16_t) Motor[i].SetPoint;
+				}
+				esc.esc[i].esc_temperature = (uint16_t) Motor[i].Temperature;
+				esc.esc[i].esc_state = (uint16_t) Motor[i].State;
+				esc.esc[i].esc_errorcount = (uint16_t) 0;
+			}
+
+			orb_publish(ORB_ID(esc_status), _t_esc_status, &esc);
+		}
+
 	}
 
+	//::close(_t_esc_status);
 	::close(_t_actuators);
 	::close(_t_actuators_effective);
 	::close(_t_actuator_safety);
@@ -716,17 +776,17 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput)
 			fprintf(stderr, "[mkblctrl] blctrl[%i] : found=%i\tversion=%i\tcurrent=%i\tmaxpwm=%i\ttemperature=%i\n", i, Motor[i].State, Motor[i].Version, Motor[i].Current, Motor[i].MaxPWM, Motor[i].Temperature);
 		}
 
-		if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
-			_task_should_exit = true;
+		
+		if(!_overrideSecurityChecks) {
+			if (foundMotorCount != 4 && foundMotorCount != 6 && foundMotorCount != 8) {
+				_task_should_exit = true;
+			}
 		}
 	}
 
 	return foundMotorCount;
 }
 
-
-
-
 int
 MK::mk_servo_set(unsigned int chan, short val)
 {
@@ -739,17 +799,15 @@ MK::mk_servo_set(unsigned int chan, short val)
 
 	tmpVal = val;
 
-	if (tmpVal > 1024) {
-		tmpVal = 1024;
+	if (tmpVal > 2047) {
+		tmpVal = 2047;
 
 	} else if (tmpVal < 0) {
 		tmpVal = 0;
 	}
 
-	Motor[chan].SetPoint = (uint8_t)(tmpVal / 4);
-	//Motor[chan].SetPointLowerBits = (uint8_t) tmpVal % 4;
-
-	Motor[chan].SetPointLowerBits = 0;
+	Motor[chan].SetPoint = (uint8_t)(tmpVal>>3)&0xff;
+	Motor[chan].SetPointLowerBits = ((uint8_t)tmpVal%8)&0x07;
 
 	if (_armed == false) {
 		Motor[chan].SetPoint = 0;
@@ -1015,8 +1073,7 @@ MK::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 	case PWM_SERVO_SET(0) ... PWM_SERVO_SET(_max_actuators - 1):
 		if (arg < 2150) {
 			Motor[cmd - PWM_SERVO_SET(0)].RawPwmValue = (unsigned short)arg;
-			mk_servo_set_value(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 1024));
-
+			mk_servo_set(cmd - PWM_SERVO_SET(0), scaling(arg, 1010, 2100, 0, 2047));
 		} else {
 			ret = -EINVAL;
 		}
@@ -1113,25 +1170,19 @@ ssize_t
 MK::write(file *filp, const char *buffer, size_t len)
 {
 	unsigned count = len / 2;
-	// loeschen uint16_t values[4];
 	uint16_t values[8];
 
-	// loeschen if (count > 4) {
-	// loeschen 	// we only have 4 PWM outputs on the FMU
-	// loeschen	count = 4;
-	// loeschen }
 	if (count > _num_outputs) {
 		// we only have 8 I2C outputs in the driver
 		count = _num_outputs;
 	}
 
-
 	// allow for misaligned values
 	memcpy(values, buffer, count * 2);
 
 	for (uint8_t i = 0; i < count; i++) {
 		Motor[i].RawPwmValue = (unsigned short)values[i];
-		mk_servo_set_value(i, scaling(values[i], 1010, 2100, 0, 1024));
+		mk_servo_set(i, scaling(values[i], 1010, 2100, 0, 2047));
 	}
 
 	return count * 2;
@@ -1283,7 +1334,7 @@ enum FrameType {
 PortMode g_port_mode;
 
 int
-mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype)
+mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest, int px4mode, int frametype, bool overrideSecurityChecks)
 {
 	uint32_t gpio_bits;
 	int shouldStop = 0;
@@ -1342,6 +1393,9 @@ mk_new_mode(PortMode new_mode, int update_rate, int motorcount, bool motortest,
 	/* motortest if enabled */
 	g_mk->set_motor_test(motortest);
 
+	/* ovveride security checks if enabled */
+	g_mk->set_overrideSecurityChecks(overrideSecurityChecks);
+
 
 	/* count used motors */
 	do {
@@ -1407,6 +1461,7 @@ mkblctrl_main(int argc, char *argv[])
 	int px4mode = MAPPING_PX4;
 	int frametype = FRAME_PLUS;	// + plus is default
 	bool motortest = false;
+	bool overrideSecurityChecks = false;
 	bool showHelp = false;
 	bool newMode = false;
 
@@ -1462,11 +1517,21 @@ mkblctrl_main(int argc, char *argv[])
 			showHelp = true;
 		}
 
+		/* look for the optional --override-security-checks  parameter */
+		if (strcmp(argv[i], "--override-security-checks") == 0) {
+			overrideSecurityChecks = true;
+			newMode = true;
+		}
+
 	}
 
 	if (showHelp) {
 		fprintf(stderr, "mkblctrl: help:\n");
-		fprintf(stderr, "  [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [-h / --help]\n");
+		fprintf(stderr, "  [-mkmode frame{+/x}] [-b i2c_bus_number] [-t motortest] [--override-security-checks] [-h / --help]\n\n");
+		fprintf(stderr, "\t -mkmode frame {+/x} \t\t Type of frame, if Mikrokopter motor order is used.\n");
+		fprintf(stderr, "\t -b i2c_bus_number \t\t Set the i2c bus where the ESCs are connected to (default 1).\n");
+		fprintf(stderr, "\t -t motortest \t\t\t Spin up once every motor in order of motoraddress. (DANGER !!!)\n");
+		fprintf(stderr, "\t --override-security-checks \t\t Disable all security checks (arming and number of ESCs). Used to test single Motors etc. (DANGER !!!)\n");
 		exit(1);
 	}
 
@@ -1484,7 +1549,7 @@ mkblctrl_main(int argc, char *argv[])
 	/* parameter set ? */
 	if (newMode) {
 		/* switch parameter */
-		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype);
+		return mk_new_mode(port_mode, pwm_update_rate_in_hz, motorcount, motortest, px4mode, frametype, overrideSecurityChecks);
 	}
 
 	/* test, etc. here g*/
diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp
index df1958186..220842536 100644
--- a/src/drivers/mpu6000/mpu6000.cpp
+++ b/src/drivers/mpu6000/mpu6000.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012 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
@@ -272,6 +272,11 @@ private:
 	 */
 	void _set_dlpf_filter(uint16_t frequency_hz);
 
+	/*
+	  set sample rate (approximate) - 1kHz to 5Hz
+	*/
+	void _set_sample_rate(uint16_t desired_sample_rate_hz);
+
 };
 
 /**
@@ -378,7 +383,8 @@ MPU6000::init()
 	up_udelay(1000);
 
 	// SAMPLE RATE
-	write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
+	//write_reg(MPUREG_SMPLRT_DIV, 0x04);     // Sample rate = 200Hz    Fsample= 1Khz/(4+1) = 200Hz
+	_set_sample_rate(200); // default sample rate = 200Hz
 	usleep(1000);
 
 	// FS & DLPF   FS=2000 deg/s, DLPF = 20Hz (low pass filter)
@@ -494,6 +500,18 @@ MPU6000::probe()
 }
 
 /*
+  set sample rate (approximate) - 1kHz to 5Hz, for both accel and gyro
+*/
+void
+MPU6000::_set_sample_rate(uint16_t desired_sample_rate_hz)
+{
+  uint8_t div = 1000 / desired_sample_rate_hz;
+  if(div>200) div=200;
+  if(div<1) div=1;
+  write_reg(MPUREG_SMPLRT_DIV, div-1);
+}
+
+/*
   set the DLPF filter frequency. This affects both accel and gyro.
  */
 void
@@ -644,8 +662,8 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case ACCELIOCSSAMPLERATE:
 	case ACCELIOCGSAMPLERATE:
-		/* XXX not implemented */
-		return -EINVAL;
+	  _set_sample_rate(arg);
+	  return OK;
 
 	case ACCELIOCSLOWPASS:
 	case ACCELIOCGLOWPASS:
@@ -702,8 +720,8 @@ MPU6000::gyro_ioctl(struct file *filp, int cmd, unsigned long arg)
 
 	case GYROIOCSSAMPLERATE:
 	case GYROIOCGSAMPLERATE:
-		/* XXX not implemented */
-		return -EINVAL;
+	  _set_sample_rate(arg);
+	  return OK;
 
 	case GYROIOCSLOWPASS:
 	case GYROIOCGLOWPASS:
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 05951f74c..d0499d2fd 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -167,7 +167,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 df2f18270..94f231821 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -145,6 +145,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:
 	// XXX
 	unsigned		_max_actuators;
@@ -190,6 +200,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[]);
@@ -330,7 +346,7 @@ PX4IO	*g_dev;
 }
 
 PX4IO::PX4IO() :
-	I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
+	I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000),
 	_max_actuators(0),
 	_max_controls(0),
 	_max_rc_input(0),
@@ -356,7 +372,8 @@ PX4IO::PX4IO() :
 	_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;
@@ -762,8 +779,6 @@ PX4IO::io_set_control_state()
 int
 PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len)
 {
-	uint16_t 		regs[_max_actuators];
-
 	if (len > _max_actuators)
 		/* fail with error */
 		return E2BIG;
@@ -1399,14 +1414,15 @@ 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_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"),
 		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_RC_DSM)   ? " DSM" : ""),
+		(((flags & PX4IO_P_STATUS_FLAGS_RC_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" : ""),
@@ -1506,7 +1522,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;
 
@@ -1558,6 +1575,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 */
@@ -1600,18 +1637,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:
@@ -1748,10 +1798,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;
@@ -1760,7 +1865,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");
@@ -2044,7 +2149,7 @@ px4io_main(int argc, char *argv[])
 			 * We can cheat and call the driver directly, as it
 		 	 * doesn't reference filp in ioctl()
 			 */
-			g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1);
+			g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1);
 		} else {
 			errx(1, "not loaded");
 		}
@@ -2088,7 +2193,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);
@@ -2162,6 +2267,9 @@ px4io_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "monitor"))
 		monitor();
 
+	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', 'min, 'max', 'idle' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug',\n 'recovery', 'limit', 'current', 'failsafe', 'min, 'max',\n 'idle', 'bind' or 'update'");
 }
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index ae33a3702..e29f76877 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -515,7 +515,7 @@ usage(const char *reason)
  * Makefile does only apply to this management task.
  *
  * The actual stack size should be set in the call
- * to task_create().
+ * to task_spawn_cmd().
  */
 int ex_fixedwing_control_main(int argc, char *argv[])
 {
diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c
index ecc133f63..0b9404582 100644
--- a/src/examples/flow_position_control/flow_position_control_main.c
+++ b/src/examples/flow_position_control/flow_position_control_main.c
@@ -102,7 +102,7 @@ usage(const char *reason)
  * Makefile does only apply to this management task.
  * 
  * The actual stack size should be set in the call
- * to task_spawn().
+ * to task_spawn_cmd().
  */
 int flow_position_control_main(int argc, char *argv[])
 {
@@ -119,7 +119,7 @@ int flow_position_control_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn("flow_position_control",
+		deamon_task = task_spawn_cmd("flow_position_control",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 6,
 					 4096,
diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index 8f84307a7..557fdf37c 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -91,7 +91,7 @@ static void usage(const char *reason)
  * Makefile does only apply to this management task.
  *
  * The actual stack size should be set in the call
- * to task_create().
+ * to task_spawn_cmd().
  */
 int flow_position_estimator_main(int argc, char *argv[])
 {
@@ -108,7 +108,7 @@ int flow_position_estimator_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		daemon_task = task_spawn("flow_position_estimator",
+		daemon_task = task_spawn_cmd("flow_position_estimator",
 					 SCHED_RR,
 					 SCHED_PRIORITY_MAX - 5,
 					 4096,
diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c
index 4f60e34f2..8032a6264 100644
--- a/src/examples/flow_speed_control/flow_speed_control_main.c
+++ b/src/examples/flow_speed_control/flow_speed_control_main.c
@@ -100,7 +100,7 @@ usage(const char *reason)
  * Makefile does only apply to this management task.
  * 
  * The actual stack size should be set in the call
- * to task_spawn().
+ * to task_spawn_cmd().
  */
 int flow_speed_control_main(int argc, char *argv[])
 {
@@ -117,7 +117,7 @@ int flow_speed_control_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn("flow_speed_control",
+		deamon_task = task_spawn_cmd("flow_speed_control",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 6,
 					 4096,
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 9d52d4f1c..2b0b3a415 100644
--- a/src/modules/commander/commander.c
+++ b/src/modules/commander/commander.c
@@ -917,6 +917,7 @@ int commander_thread_main(int argc, char *argv[])
 	/* XXX use this! */
 	//uint64_t failsave_ll_start_time = 0;
 
+	enum VEHICLE_MANUAL_SAS_MODE	manual_sas_mode;
 	bool state_changed = true;
 	bool param_init_forced = true;
 
@@ -1567,17 +1568,23 @@ int commander_thread_main(int argc, char *argv[])
 				*/
 				// printf("checking\n");
 				if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) {
+
 					if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 
 						if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) ||
 						 (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) ||
 						 (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)
 						) {
-							arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
-							tune_positive();
+							if (current_status.mode_switch != MODE_SWITCH_MANUAL) {
+								mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first");
+								tune_negative();
+							} else {
+								arming_state_transition(status_pub, &current_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd);
+								tune_positive();
+							}
 							
 						} else {
-							mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed");
+							mavlink_log_critical(mavlink_fd, "DISARM not allowed");
 							tune_negative();
 						}
 						stick_off_counter = 0;
@@ -1589,7 +1596,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) {
 						arming_state_transition(status_pub, &current_status, ARMING_STATE_ARMED, safety_pub, &safety, 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 618095d0b..97b585cb7 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -54,10 +54,12 @@
 #include <uORB/topics/actuator_safety.h>
 #include <poll.h>
 #include <drivers/drv_gpio.h>
+#include <modules/px4iofirmware/protocol.h>
 
 struct gpio_led_s {
 	struct work_s work;
 	int gpio_fd;
+	bool use_io;
 	int pin;
 	struct vehicle_status_s status;
 	int vehicle_status_sub;
@@ -78,51 +80,97 @@ void gpio_led_cycle(FAR void *arg);
 
 int gpio_led_main(int argc, char *argv[])
 {
-	int pin = GPIO_EXT_1;
-
 	if (argc < 2) {
-		errx(1, "no argument provided. Try 'start' or 'stop' [-p 1/2]");
+		errx(1, "usage: gpio_led {start|stop} [-p <1|2|a1|a2|r1|r2>]\n"
+		     "\t-p\tUse pin:\n"
+		     "\t\t1\tPX4FMU GPIO_EXT1 (default)\n"
+		     "\t\t2\tPX4FMU GPIO_EXT2\n"
+		     "\t\ta1\tPX4IO ACC1\n"
+		     "\t\ta2\tPX4IO ACC2\n"
+		     "\t\tr1\tPX4IO RELAY1\n"
+		     "\t\tr2\tPX4IO RELAY2");
 
 	} else {
 
-		/* START COMMAND HANDLING */
 		if (!strcmp(argv[1], "start")) {
+			if (gpio_led_started) {
+				errx(1, "already running");
+			}
+
+			bool use_io = false;
+			int pin = GPIO_EXT_1;
 
 			if (argc > 2) {
-				if (!strcmp(argv[1], "-p")) {
-					if (!strcmp(argv[2], "1")) {
+				if (!strcmp(argv[2], "-p")) {
+					if (!strcmp(argv[3], "1")) {
+						use_io = false;
 						pin = GPIO_EXT_1;
 
-					} else if (!strcmp(argv[2], "2")) {
+					} else if (!strcmp(argv[3], "2")) {
+						use_io = false;
 						pin = GPIO_EXT_2;
 
+					} else if (!strcmp(argv[3], "a1")) {
+						use_io = true;
+						pin = PX4IO_ACC1;
+
+					} else if (!strcmp(argv[3], "a2")) {
+						use_io = true;
+						pin = PX4IO_ACC2;
+
+					} else if (!strcmp(argv[3], "r1")) {
+						use_io = true;
+						pin = PX4IO_RELAY1;
+
+					} else if (!strcmp(argv[3], "r2")) {
+						use_io = true;
+						pin = PX4IO_RELAY2;
+
 					} else {
-						warnx("[gpio_led] Unsupported pin: %s\n", argv[2]);
-						exit(1);
+						errx(1, "unsupported pin: %s", argv[3]);
 					}
 				}
 			}
 
 			memset(&gpio_led_data, 0, sizeof(gpio_led_data));
+			gpio_led_data.use_io = use_io;
 			gpio_led_data.pin = pin;
 			int ret = work_queue(LPWORK, &gpio_led_data.work, gpio_led_start, &gpio_led_data, 0);
 
 			if (ret != 0) {
-				warnx("[gpio_led] Failed to queue work: %d\n", ret);
-				exit(1);
+				errx(1, "failed to queue work: %d", ret);
 
 			} else {
 				gpio_led_started = true;
+				char pin_name[24];
+
+				if (use_io) {
+					if (pin & (PX4IO_ACC1 | PX4IO_ACC2)) {
+						sprintf(pin_name, "PX4IO ACC%i", (pin >> 3));
+
+					} else {
+						sprintf(pin_name, "PX4IO RELAY%i", pin);
+					}
+
+				} else {
+					sprintf(pin_name, "PX4FMU GPIO_EXT%i", pin);
+
+				}
+
+				warnx("start, using pin: %s", pin_name);
 			}
 
 			exit(0);
 
-			/* STOP COMMAND HANDLING */
 
 		} else if (!strcmp(argv[1], "stop")) {
-			gpio_led_started = false;
+			if (gpio_led_started) {
+				gpio_led_started = false;
+				warnx("stop");
 
-			/* INVALID COMMAND */
+			} else {
+				errx(1, "not running");
+			}
 
 		} else {
 			errx(1, "unrecognized command '%s', only supporting 'start' or 'stop'", argv[1]);
@@ -134,15 +182,26 @@ void gpio_led_start(FAR void *arg)
 {
 	FAR struct gpio_led_s *priv = (FAR struct gpio_led_s *)arg;
 
+	char *gpio_dev;
+
+	if (priv->use_io) {
+		gpio_dev = PX4IO_DEVICE_PATH;
+	} else {
+		gpio_dev = PX4FMU_DEVICE_PATH;
+	}
+
 	/* open GPIO device */
-	priv->gpio_fd = open(GPIO_DEVICE_PATH, 0);
+	priv->gpio_fd = open(gpio_dev, 0);
 
 	if (priv->gpio_fd < 0) {
-		warnx("[gpio_led] GPIO: open fail\n");
+		// TODO find way to print errors
+		//printf("gpio_led: GPIO device \"%s\" open fail\n", gpio_dev);
+		gpio_led_started = false;
 		return;
 	}
 
 	/* 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 */
@@ -153,11 +212,11 @@ void gpio_led_start(FAR void *arg)
 	int ret = work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, 0);
 
 	if (ret != 0) {
-		warnx("[gpio_led] Failed to queue work: %d\n", ret);
+		// TODO find way to print errors
+		//printf("gpio_led: failed to queue work: %d\n", ret);
+		gpio_led_started = false;
 		return;
 	}
-
-	warnx("[gpio_led] Started, using pin GPIO_EXT%i\n", priv->pin);
 }
 
 void gpio_led_cycle(FAR void *arg)
@@ -207,7 +266,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);
 		}
@@ -218,7 +276,12 @@ void gpio_led_cycle(FAR void *arg)
 	if (priv->counter > 5)
 		priv->counter = 0;
 
-	/* repeat cycle at 5 Hz*/
-	if (gpio_led_started)
+	/* repeat cycle at 5 Hz */
+	if (gpio_led_started) {
 		work_queue(LPWORK, &priv->work, gpio_led_cycle, priv, USEC2TICK(200000));
+
+	} else {
+		/* switch off LED on stop */
+		ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
+	}
 }
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index f6c371c7c..a2758a45c 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -49,7 +49,6 @@
 #include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
@@ -464,7 +463,7 @@ int mavlink_open_uart(int baud, const char *uart_name, struct termios *uart_conf
 }
 
 void
-mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
+mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length)
 {
 	write(uart, ch, (size_t)(sizeof(uint8_t) * length));
 }
@@ -472,7 +471,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, uint8_t *ch, int length)
 /*
  * Internal function to give access to the channel status for each channel
  */
-mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
+extern mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
 {
 	static mavlink_status_t m_mavlink_status[MAVLINK_COMM_NUM_BUFFERS];
 	return &m_mavlink_status[channel];
@@ -481,7 +480,7 @@ mavlink_status_t *mavlink_get_channel_status(uint8_t channel)
 /*
  * Internal function to give access to the channel buffer for each channel
  */
-mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
+extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t channel)
 {
 	static mavlink_message_t m_mavlink_buffer[MAVLINK_COMM_NUM_BUFFERS];
 	return &m_mavlink_buffer[channel];
diff --git a/src/modules/mavlink/mavlink_bridge_header.h b/src/modules/mavlink/mavlink_bridge_header.h
index 0010bb341..149efda60 100644
--- a/src/modules/mavlink/mavlink_bridge_header.h
+++ b/src/modules/mavlink/mavlink_bridge_header.h
@@ -73,9 +73,11 @@ extern mavlink_system_t mavlink_system;
  * @param chan MAVLink channel to use, usually MAVLINK_COMM_0 = UART0
  * @param ch Character to send
  */
-extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int length);
+extern void mavlink_send_uart_bytes(mavlink_channel_t chan, const uint8_t *ch, int length);
 
-mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
-mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
+extern mavlink_status_t *mavlink_get_channel_status(uint8_t chan);
+extern mavlink_message_t *mavlink_get_channel_buffer(uint8_t chan);
+
+#include <v1.0/common/mavlink.h>
 
 #endif /* MAVLINK_BRIDGE_HEADER_H */
diff --git a/src/modules/mavlink/mavlink_parameters.c b/src/modules/mavlink/mavlink_parameters.c
index 819f3441b..18ca7a854 100644
--- a/src/modules/mavlink/mavlink_parameters.c
+++ b/src/modules/mavlink/mavlink_parameters.c
@@ -40,7 +40,6 @@
  */
 
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include "mavlink_parameters.h"
 #include <uORB/uORB.h>
 #include "math.h" /* isinf / isnan checks */
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 33ac14860..01bbabd46 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -49,7 +49,6 @@
 #include <fcntl.h>
 #include <mqueue.h>
 #include <string.h>
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
@@ -503,7 +502,6 @@ handle_message(mavlink_message_t *msg)
 			} else {
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
-			warnx("IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
 
 			hil_global_pos.lat = hil_state.lat;
 			hil_global_pos.lon = hil_state.lon;
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index 4b010dd59..be88b8794 100644
--- a/src/modules/mavlink/missionlib.c
+++ b/src/modules/mavlink/missionlib.c
@@ -48,7 +48,6 @@
 #include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
diff --git a/src/modules/mavlink/missionlib.h b/src/modules/mavlink/missionlib.h
index c2ca735b3..c7d8f90c5 100644
--- a/src/modules/mavlink/missionlib.h
+++ b/src/modules/mavlink/missionlib.h
@@ -39,7 +39,7 @@
 
 #pragma once
 
-#include <v1.0/common/mavlink.h>
+#include "mavlink_bridge_header.h"
 
 //extern void	mavlink_wpm_send_message(mavlink_message_t *msg);
 //extern void	mavlink_wpm_send_gcs_string(const char *string);
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index a9a611998..95bc8fdc0 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -47,7 +47,6 @@
 #include <fcntl.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index cefcca468..eea928a17 100644
--- a/src/modules/mavlink/waypoints.c
+++ b/src/modules/mavlink/waypoints.c
@@ -45,6 +45,7 @@
 #include <unistd.h>
 #include <stdio.h>
 
+#include "mavlink_bridge_header.h"
 #include "missionlib.h"
 #include "waypoints.h"
 #include "util.h"
@@ -372,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/mavlink/waypoints.h b/src/modules/mavlink/waypoints.h
index c32ab32e5..96a0ecd30 100644
--- a/src/modules/mavlink/waypoints.h
+++ b/src/modules/mavlink/waypoints.h
@@ -47,11 +47,11 @@
 
 #include <v1.0/mavlink_types.h>
 
-#ifndef MAVLINK_SEND_UART_BYTES
-#define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
-#endif
-extern mavlink_system_t mavlink_system;
-#include <v1.0/common/mavlink.h>
+// #ifndef MAVLINK_SEND_UART_BYTES
+// #define MAVLINK_SEND_UART_BYTES(chan, buffer, len) mavlink_send_uart_bytes(chan, buffer, len)
+// #endif
+//extern mavlink_system_t mavlink_system;
+#include "mavlink_bridge_header.h"
 #include <stdbool.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_local_position.h>
diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c
index 35d53b880..2d5a64ee8 100644
--- a/src/modules/mavlink_onboard/mavlink.c
+++ b/src/modules/mavlink_onboard/mavlink.c
@@ -49,7 +49,6 @@
 #include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
diff --git a/src/modules/mavlink_onboard/mavlink_bridge_header.h b/src/modules/mavlink_onboard/mavlink_bridge_header.h
index 3ad3bb617..b72bbb2b1 100644
--- a/src/modules/mavlink_onboard/mavlink_bridge_header.h
+++ b/src/modules/mavlink_onboard/mavlink_bridge_header.h
@@ -78,4 +78,6 @@ extern void mavlink_send_uart_bytes(mavlink_channel_t chan, uint8_t *ch, int len
 mavlink_status_t* mavlink_get_channel_status(uint8_t chan);
 mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan);
 
+#include <v1.0/common/mavlink.h>
+
 #endif /* MAVLINK_BRIDGE_HEADER_H */
diff --git a/src/modules/mavlink_onboard/mavlink_receiver.c b/src/modules/mavlink_onboard/mavlink_receiver.c
index 2d406fb9f..0236e6126 100644
--- a/src/modules/mavlink_onboard/mavlink_receiver.c
+++ b/src/modules/mavlink_onboard/mavlink_receiver.c
@@ -50,7 +50,6 @@
 #include <mqueue.h>
 #include <string.h>
 #include "mavlink_bridge_header.h"
-#include <v1.0/common/mavlink.h>
 #include <drivers/drv_hrt.h>
 #include <time.h>
 #include <float.h>
diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c
index 3cf9ca149..43d96fb06 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 0e477a200..ca58ba0eb 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -109,6 +109,7 @@
 #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_SAFETY_OFF		(1 << 12) /* safety is off */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 13) /* 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) /* VBatt is very close to regulator dropout */
@@ -161,7 +162,22 @@
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_PWM_ALTRATE		4	/* 'high' PWM frame output rate in Hz */
 #define PX4IO_P_SETUP_RELAYS			5	/* bitmask of relay/switch outputs, 0 = off, 1 = on */
+
+/* px4io relay bit definitions */
+#define PX4IO_RELAY1	(1<<0)
+#define PX4IO_RELAY2	(1<<1)
+#define PX4IO_ACC1		(1<<2)
+#define PX4IO_ACC2		(1<<3)
+
 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* battery voltage correction factor (float) */
+#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
+};
 #define PX4IO_P_SETUP_SET_DEBUG			9	/* debug level for IO board */
 
 /* autopilot control values, -10000..10000 */
diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h
index 042e7fe66..a020f8704 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -187,6 +187,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 bd13f3b7d..bf895d31f 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -446,10 +446,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_RELAYS:
 			value &= PX4IO_P_SETUP_RELAYS_VALID;
 			r_setup_relays = value;
-			POWER_RELAY1(value & (1 << 0) ? 1 : 0);
-			POWER_RELAY2(value & (1 << 1) ? 1 : 0);
-			POWER_ACC1(value & (1 << 2) ? 1 : 0);
-			POWER_ACC2(value & (1 << 3) ? 1 : 0);
+			POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0);
+			POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0);
+			POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0);
+			POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0);
 			break;
 
 		case PX4IO_P_SETUP_SET_DEBUG:
@@ -457,6 +457,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 31da81f5c..80ee3adee 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -73,6 +73,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/vehicle_control_debug.h>
@@ -81,6 +82,7 @@
 #include <uORB/topics/differential_pressure.h>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/rc_channels.h>
+#include <uORB/topics/esc_status.h>
 
 #include <systemlib/systemlib.h>
 
@@ -614,9 +616,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 		errx(1, "can't allocate log buffer, exiting.");
 	}
 
-	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
-	/* number of messages */
-	const ssize_t fdsc = 20;
+	/* --- IMPORTANT: DEFINE MAX NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
+	/* max number of messages */
+	const ssize_t fdsc = 21;
 
 	/* Sanity check variable and index */
 	ssize_t fdsc_count = 0;
@@ -642,6 +644,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 vehicle_control_debug_s control_debug;
@@ -649,6 +652,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		struct rc_channels_s rc;
 		struct differential_pressure_s diff_pres;
 		struct airspeed_s airspeed;
+		struct esc_status_s esc;
 	} buf;
 	memset(&buf, 0, sizeof(buf));
 
@@ -666,12 +670,14 @@ 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 control_debug_sub;
 		int flow_sub;
 		int rc_sub;
 		int airspeed_sub;
+		int esc_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -696,6 +702,8 @@ 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 = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -787,6 +795,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;
@@ -817,6 +831,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 	fds[fdsc_count].events = POLLIN;
 	fdsc_count++;	
 
+	/* --- ESCs --- */
+	subs.esc_sub = orb_subscribe(ORB_ID(esc_status));
+	fds[fdsc_count].fd = subs.esc_sub;
+	fds[fdsc_count].events = POLLIN;
+	fdsc_count++;
+
 	/* WARNING: If you get the error message below,
 	 * then the number of registered messages (fdsc)
 	 * differs from the number of messages in the above list.
@@ -1108,6 +1128,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);
@@ -1167,6 +1206,28 @@ int sdlog2_thread_main(int argc, char *argv[])
 				LOGBUFFER_WRITE_AND_COUNT(AIRS);
 			}
 
+			/* --- ESCs --- */
+			if (fds[ifds++].revents & POLLIN) {
+				orb_copy(ORB_ID(esc_status), subs.esc_sub, &buf.esc);
+				for (uint8_t i=0; i<buf.esc.esc_count; i++)
+				{
+					log_msg.msg_type = LOG_ESC_MSG;
+					log_msg.body.log_ESC.counter = buf.esc.counter;
+					log_msg.body.log_ESC.esc_count = buf.esc.esc_count;
+					log_msg.body.log_ESC.esc_connectiontype = buf.esc.esc_connectiontype;
+					log_msg.body.log_ESC.esc_num = i;
+					log_msg.body.log_ESC.esc_address = buf.esc.esc[i].esc_address;
+					log_msg.body.log_ESC.esc_version = buf.esc.esc[i].esc_version;
+					log_msg.body.log_ESC.esc_voltage = buf.esc.esc[i].esc_voltage;
+					log_msg.body.log_ESC.esc_current = buf.esc.esc[i].esc_current;
+					log_msg.body.log_ESC.esc_rpm = buf.esc.esc[i].esc_rpm;
+					log_msg.body.log_ESC.esc_temperature = buf.esc.esc[i].esc_temperature;
+					log_msg.body.log_ESC.esc_setpoint = buf.esc.esc[i].esc_setpoint;
+					log_msg.body.log_ESC.esc_setpoint_raw = buf.esc.esc[i].esc_setpoint_raw;
+					LOGBUFFER_WRITE_AND_COUNT(ESC);
+				}
+			}
+
 #ifdef SDLOG2_DEBUG
 				printf("fill rp=%i wp=%i count=%i\n", lb.read_ptr, lb.write_ptr, logbuffer_count(&lb));
 #endif
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 358637f93..9be96a62e 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -152,15 +152,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;
 };
 
 /* --- CTRL - CONTROL DEBUG --- */
@@ -229,6 +229,41 @@ struct log_GPOS_s {
 	float vel_e;
 	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 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;
+	uint16_t esc_voltage;
+	uint16_t esc_current;
+	uint16_t esc_rpm;
+	uint16_t esc_temperature;
+	float    esc_setpoint;
+	uint16_t esc_setpoint_raw;
+};
+
 #pragma pack(pop)
 
 /* construct list of all message formats */
@@ -251,6 +286,8 @@ 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"),
 };
 
 static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s);
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 7077972aa..3ed9efc8b 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 49a1da83b..acc0c2717 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -228,6 +228,8 @@ private:
 		float rc_scale_flaps;
 
 		float battery_voltage_scaling;
+
+		int   rc_rl1_DSM_VCC_control;
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -274,6 +276,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 */
 
 
@@ -509,6 +513,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();
 }
@@ -715,6 +722,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/systemlib/airspeed.c b/src/modules/systemlib/airspeed.c
index 15bb833a9..e01cc4dda 100644
--- a/src/modules/systemlib/airspeed.c
+++ b/src/modules/systemlib/airspeed.c
@@ -62,7 +62,7 @@ float calc_indicated_airspeed(float differential_pressure)
 	if (differential_pressure > 0) {
 		return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
 	} else {
-		return -sqrtf((2.0f*fabs(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
+		return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
 	}
 
 }
@@ -106,6 +106,6 @@ float calc_true_airspeed(float total_pressure, float static_pressure, float temp
 		return sqrtf((2.0f*(pressure_difference)) / density);
 	} else
 	{
-		return -sqrtf((2.0f*fabs(pressure_difference)) / density);
+		return -sqrtf((2.0f*fabsf(pressure_difference)) / density);
 	}
 }
diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index 8fdff8ac0..afc5b072c 100644
--- a/src/modules/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
@@ -71,8 +71,6 @@ extern FAR struct _TCB *sched_gettcb(pid_t pid);
 
 void cpuload_initialize_once()
 {
-//	if (!system_load.initialized)
-//	{
 	system_load.start_time = hrt_absolute_time();
 	int i;
 
@@ -80,27 +78,29 @@ void cpuload_initialize_once()
 		system_load.tasks[i].valid = false;
 	}
 
-	system_load.total_count = 0;
-
 	uint64_t now = hrt_absolute_time();
 
-	/* initialize idle thread statically */
-	system_load.tasks[0].start_time = now;
-	system_load.tasks[0].total_runtime = 0;
-	system_load.tasks[0].curr_start_time = 0;
-	system_load.tasks[0].tcb = sched_gettcb(0);
-	system_load.tasks[0].valid = true;
-	system_load.total_count++;
-
-	/* initialize init thread statically */
-	system_load.tasks[1].start_time = now;
-	system_load.tasks[1].total_runtime = 0;
-	system_load.tasks[1].curr_start_time = 0;
-	system_load.tasks[1].tcb = sched_gettcb(1);
-	system_load.tasks[1].valid = true;
-	/* count init thread */
-	system_load.total_count++;
-	//	}
+	int static_tasks_count = 2;	// there are at least 2 threads that should be initialized statically - "idle" and "init"
+
+#ifdef CONFIG_PAGING
+	static_tasks_count++;	// include paging thread in initialization
+#endif /* CONFIG_PAGING */
+#if CONFIG_SCHED_WORKQUEUE
+	static_tasks_count++;	// include high priority work0 thread in initialization
+#endif /* CONFIG_SCHED_WORKQUEUE */
+#if CONFIG_SCHED_LPWORK
+	static_tasks_count++;	// include low priority work1 thread in initialization
+#endif /* CONFIG_SCHED_WORKQUEUE */
+
+	// perform static initialization of "system" threads
+	for (system_load.total_count = 0; system_load.total_count < static_tasks_count; system_load.total_count++)
+	{
+		system_load.tasks[system_load.total_count].start_time = now;
+		system_load.tasks[system_load.total_count].total_runtime = 0;
+		system_load.tasks[system_load.total_count].curr_start_time = 0;
+		system_load.tasks[system_load.total_count].tcb = sched_gettcb(system_load.total_count);	// it is assumed that these static threads have consecutive PIDs
+		system_load.tasks[system_load.total_count].valid = true;
+	}
 }
 
 void sched_note_start(FAR struct tcb_s *tcb)
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index 8ae47ba8b..a061fe676 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -177,3 +177,6 @@ ORB_DEFINE(debug_key_value, struct debug_key_value_s);
 
 #include "topics/navigation_capabilities.h"
 ORB_DEFINE(navigation_capabilities, struct navigation_capabilities_s);
+
+#include "topics/esc_status.h"
+ORB_DEFINE(esc_status, struct esc_status_s);
diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h
new file mode 100644
index 000000000..00cf59b28
--- /dev/null
+++ b/src/modules/uORB/topics/esc_status.h
@@ -0,0 +1,115 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: @author Marco Bauer <marco@wtns.de>
+ *
+ * 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 esc_status.h
+ * Definition of the esc_status uORB topic.
+ *
+ * Published the state machine and the system status bitfields
+ * (see SYS_STATUS mavlink message), published only by commander app.
+ *
+ * All apps should write to subsystem_info:
+ *
+ *  (any app) --> subsystem_info (published) --> (commander app state machine)  --> esc_status --> (mavlink app)
+ */
+
+#ifndef ESC_STATUS_H_
+#define ESC_STATUS_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics @{
+ */
+
+/**
+ * The number of ESCs supported.
+ * Current (Q2/2013) we support 8 ESCs,
+ */
+#define CONNECTED_ESC_MAX   8
+
+enum ESC_VENDOR {
+	ESC_VENDOR_GENERIC = 0,					/**< generic ESC */
+	ESC_VENDOR_MIKROKOPTER,					/**< Mikrokopter */
+	ESC_VENDOR_GRAUPNER_HOTT				/**< Graupner HoTT ESC */	
+};
+
+enum ESC_CONNECTION_TYPE {
+	ESC_CONNECTION_TYPE_PPM = 0,			/**< Traditional PPM ESC */
+	ESC_CONNECTION_TYPE_SERIAL,			/**< Serial Bus connected ESC */
+	ESC_CONNECTION_TYPE_ONESHOOT,			/**< One Shoot PPM */
+	ESC_CONNECTION_TYPE_I2C,				/**< I2C */
+	ESC_CONNECTION_TYPE_CAN					/**< CAN-Bus */
+};
+
+/**
+ *
+ */
+struct esc_status_s
+{
+	/* use of a counter and timestamp recommended (but not necessary) */
+
+	uint16_t counter;   		/**< incremented by the writing thread everytime new data is stored */
+	uint64_t timestamp; 		/**< in microseconds since system start, is set whenever the writing thread stores new data */
+	
+	uint8_t esc_count;			/**< number of connected ESCs */
+	enum ESC_CONNECTION_TYPE esc_connectiontype;	/**< how ESCs connected to the system */
+	
+	struct {
+		uint16_t esc_address;			/**< Address of current ESC (in most cases 1-8 / must be set by driver) */
+		enum ESC_VENDOR esc_vendor;		/**< Vendor of current ESC */
+		uint16_t esc_version;			/**< Version of current ESC - if supported */
+		uint16_t esc_voltage;			/**< Voltage measured from current ESC - if supported */
+		uint16_t esc_current;			/**< Current measured from current ESC (100mA steps) - if supported */
+		uint16_t esc_rpm;				/**< RPM measured from current ESC - if supported */
+		uint16_t esc_temperature;		/**< Temperature measured from current ESC - if supported */
+		float    esc_setpoint;			/**< setpoint of current ESC */
+		uint16_t esc_setpoint_raw;		/**< setpoint of current ESC (Value sent to ESC) */
+		uint16_t esc_state;				/**< State of ESC - depend on Vendor */
+		uint16_t esc_errorcount;		/**< Number of reported errors by ESC - if supported */
+	} esc[CONNECTED_ESC_MAX];
+
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+//ORB_DECLARE(esc_status);
+ORB_DECLARE_OPTIONAL(esc_status);
+
+#endif
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");
diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c
index efe62685c..0d064a05e 100644
--- a/src/systemcmds/top/top.c
+++ b/src/systemcmds/top/top.c
@@ -51,19 +51,46 @@
 #include <systemlib/cpuload.h>
 #include <drivers/drv_hrt.h>
 
+#define CL "\033[K" // clear line
+
 /**
  * Start the top application.
  */
-__EXPORT int top_main(int argc, char *argv[]);
+__EXPORT int top_main(void);
 
 extern struct system_load_s system_load;
 
-bool top_sigusr1_rcvd = false;
-
-int top_main(int argc, char *argv[])
+static const char *
+tstate_name(const tstate_t s)
 {
-	int t;
+	switch (s) {
+	case TSTATE_TASK_INVALID:    return "init";
+
+	case TSTATE_TASK_PENDING:    return "PEND";
+	case TSTATE_TASK_READYTORUN: return "READY";
+	case TSTATE_TASK_RUNNING:    return "RUN";
+
+	case TSTATE_TASK_INACTIVE:   return "inact";
+	case TSTATE_WAIT_SEM:        return "w:sem";
+#ifndef CONFIG_DISABLE_SIGNALS
+	case TSTATE_WAIT_SIG:        return "w:sig";
+#endif
+#ifndef CONFIG_DISABLE_MQUEUE
+	case TSTATE_WAIT_MQNOTEMPTY: return "w:mqe";
+	case TSTATE_WAIT_MQNOTFULL:  return "w:mqf";
+#endif
+#ifdef CONFIG_PAGING
+	case TSTATE_WAIT_PAGEFILL:   return "w:pgf";
+#endif
+
+	default:
+		return "ERROR";
+	}
+}
 
+int
+top_main(void)
+{
 	uint64_t total_user_time = 0;
 
 	int running_count = 0;
@@ -75,7 +102,7 @@ int top_main(int argc, char *argv[])
 	uint64_t last_times[CONFIG_MAX_TASKS];
 	float curr_loads[CONFIG_MAX_TASKS];
 
-	for (t = 0; t < CONFIG_MAX_TASKS; t++)
+	for (int t = 0; t < CONFIG_MAX_TASKS; t++)
 		last_times[t] = 0;
 
 	float interval_time_ms_inv = 0.f;
@@ -83,16 +110,16 @@ int top_main(int argc, char *argv[])
 	/* Open console directly to grab CTRL-C signal */
 	int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY);
 
-	while (true)
-//	for (t = 0; t < 10; t++)
-	{
-		int i;
+	/* clear screen */
+	printf("\033[2J");
 
-		uint64_t curr_time_ms = (hrt_absolute_time() / 1000LLU);
-		unsigned int curr_time_s = curr_time_ms / 1000LLU;
+	for (;;) {
+		int   i;
+		uint64_t curr_time_us;
+		uint64_t idle_time_us;
 
-		uint64_t idle_time_total_ms = (system_load.tasks[0].total_runtime / 1000LLU);
-		unsigned int idle_time_total_s = idle_time_total_ms / 1000LLU;
+		curr_time_us = hrt_absolute_time();
+		idle_time_us = system_load.tasks[0].total_runtime;
 
 		if (new_time > interval_start_time)
 			interval_time_ms_inv = 1.f / ((float)((new_time - interval_start_time) / 1000));
@@ -102,7 +129,38 @@ int top_main(int argc, char *argv[])
 		total_user_time = 0;
 
 		for (i = 0; i < CONFIG_MAX_TASKS; i++) {
-			uint64_t interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 && system_load.tasks[i].total_runtime > last_times[i]) ? (system_load.tasks[i].total_runtime - last_times[i]) / 1000 : 0;
+			uint64_t interval_runtime;
+
+			if (system_load.tasks[i].valid) {
+				switch (system_load.tasks[i].tcb->task_state) {
+				case TSTATE_TASK_PENDING:
+				case TSTATE_TASK_READYTORUN:
+				case TSTATE_TASK_RUNNING:
+					running_count++;
+					break;
+
+				case TSTATE_TASK_INVALID:
+				case TSTATE_TASK_INACTIVE:
+				case TSTATE_WAIT_SEM:
+#ifndef CONFIG_DISABLE_SIGNALS
+				case TSTATE_WAIT_SIG:
+#endif
+#ifndef CONFIG_DISABLE_MQUEUE
+				case TSTATE_WAIT_MQNOTEMPTY:
+				case TSTATE_WAIT_MQNOTFULL:
+#endif
+#ifdef CONFIG_PAGING
+				case TSTATE_WAIT_PAGEFILL:
+#endif
+					blocked_count++;
+					break;
+				}
+			}
+
+			interval_runtime = (system_load.tasks[i].valid && last_times[i] > 0 &&
+								system_load.tasks[i].total_runtime > last_times[i])
+				? (system_load.tasks[i].total_runtime - last_times[i]) / 1000
+				: 0;
 
 			last_times[i] = system_load.tasks[i].total_runtime;
 
@@ -111,7 +169,6 @@ int top_main(int argc, char *argv[])
 
 				if (i > 0)
 					total_user_time += interval_runtime;
-
 			} else
 				curr_loads[i] = 0;
 		}
@@ -119,127 +176,99 @@ int top_main(int argc, char *argv[])
 		for (i = 0; i < CONFIG_MAX_TASKS; i++) {
 			if (system_load.tasks[i].valid && (new_time > interval_start_time)) {
 				if (system_load.tasks[i].tcb->pid == 0) {
-					float idle = curr_loads[0];
-					float task_load = (float)(total_user_time) * interval_time_ms_inv;
+					float idle;
+					float task_load;
+					float sched_load;
 
-					if (task_load > (1.f - idle)) task_load = (1.f - idle);	/* this can happen if one tasks total runtime was not computed correctly by the scheduler instrumentation TODO */
+					idle = curr_loads[0];
+					task_load = (float)(total_user_time) * interval_time_ms_inv;
 
-					float sched_load = 1.f - idle - task_load;
+					/* this can happen if one tasks total runtime was not computed
+					   correctly by the scheduler instrumentation TODO */
+					if (task_load > (1.f - idle))
+						task_load = (1.f - idle);
+
+					sched_load = 1.f - idle - task_load;
 
 					/* print system information */
-					printf("\033[H"); /* cursor home */
-					printf("\033[KProcesses: %d total, %d running, %d sleeping\n", system_load.total_count, running_count, blocked_count);
-					printf("\033[KCPU usage: %d.%02d%% tasks, %d.%02d%% sched, %d.%02d%% idle\n", (int)(task_load * 100), (int)((task_load * 10000.0f) - (int)(task_load * 100.0f) * 100), (int)(sched_load * 100), (int)((sched_load * 10000.0f) - (int)(sched_load * 100.0f) * 100), (int)(idle * 100), (int)((idle * 10000.0f) - ((int)(idle * 100)) * 100));
-					printf("\033[KUptime: %u.%03u s total, %d.%03d s idle\n\033[K\n", curr_time_s, (unsigned int)(curr_time_ms - curr_time_s * 1000LLU), idle_time_total_s, (int)(idle_time_total_ms - idle_time_total_s * 1000));
-
-					/* 34 chars command name length (32 chars plus two spaces) */
-					char header_spaces[CONFIG_TASK_NAME_SIZE + 1];
-					memset(header_spaces, ' ', CONFIG_TASK_NAME_SIZE);
-					header_spaces[CONFIG_TASK_NAME_SIZE] = '\0';
+					printf("\033[H"); /* move cursor home and clear screen */
+					printf(CL "Processes: %d total, %d running, %d sleeping\n",
+						   system_load.total_count,
+						   running_count,
+						   blocked_count);
+					printf(CL "CPU usage: %.2f%% tasks, %.2f%% sched, %.2f%% idle\n",
+						   (double)(task_load * 100.f),
+						   (double)(sched_load * 100.f),
+						   (double)(idle * 100.f));
+					printf(CL "Uptime: %.3fs total, %.3fs idle\n\n",
+						   (double)curr_time_us / 1000000.d,
+						   (double)idle_time_us / 1000000.d);
+
+					/* header for task list */
+					printf(CL "%4s %*-s %8s %6s %11s %10s %-6s\n",
+						   "PID",
+						   CONFIG_TASK_NAME_SIZE, "COMMAND",
+						   "CPU(ms)",
+						   "CPU(%)",
+						   "USED/STACK",
+						   "PRIO(BASE)",
 #if CONFIG_RR_INTERVAL > 0
-					printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\tRR SLICE\n", header_spaces);
+						   "TSLICE"
 #else
-					printf("\033[KPID\tCOMMAND%s CPU TOTAL \t%%CPU CURR \tSTACK USE\tCURR (BASE) PRIO\n", header_spaces);
-#endif
-
-				} else {
-					enum tstate_e task_state = (enum tstate_e)system_load.tasks[i].tcb->task_state;
-
-					if (task_state == TSTATE_TASK_PENDING ||
-					    task_state == TSTATE_TASK_READYTORUN ||
-					    task_state == TSTATE_TASK_RUNNING) {
-						running_count++;
-					}
-
-					if (task_state == TSTATE_TASK_INACTIVE ||      /* BLOCKED      - Initialized but not yet activated */
-					    task_state == TSTATE_WAIT_SEM           /* BLOCKED      - Waiting for a semaphore */
-#ifndef CONFIG_DISABLE_SIGNALS
-					    || task_state == TSTATE_WAIT_SIG           /* BLOCKED      - Waiting for a signal */
+						   "STATE"
 #endif
-#ifndef CONFIG_DISABLE_MQUEUE
-					    || task_state == TSTATE_WAIT_MQNOTEMPTY     /* BLOCKED      - Waiting for a MQ to become not empty. */
-					    || task_state == TSTATE_WAIT_MQNOTFULL      /* BLOCKED      - Waiting for a MQ to become not full. */
-#endif
-#ifdef CONFIG_PAGING
-					    || task_state == TSTATE_WAIT_PAGEFILL       /* BLOCKED      - Waiting for page fill */
-#endif
-					   ) {
-						blocked_count++;
-					}
-
-					char spaces[CONFIG_TASK_NAME_SIZE + 2];
-
-					/* count name len */
-					int namelen = 0;
-
-					while (namelen < CONFIG_TASK_NAME_SIZE) {
-						if (system_load.tasks[i].tcb->name[namelen] == '\0') break;
-
-						namelen++;
-					}
-
-					int s = 0;
-
-					for (s = 0; s < CONFIG_TASK_NAME_SIZE + 2 - namelen; s++) {
-						spaces[s] = ' ';
-					}
-
-					spaces[s] = '\0';
-
-					char *runtime_spaces = "  ";
+						   );
+				}
 
-					if ((system_load.tasks[i].total_runtime / 1000) < 99) {
-						runtime_spaces = "";
-					}
+				unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
+					(uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
+				unsigned stack_free = 0;
+				uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
 
-					unsigned stack_size = (uintptr_t)system_load.tasks[i].tcb->adj_stack_ptr -
-							      (uintptr_t)system_load.tasks[i].tcb->stack_alloc_ptr;
-					unsigned stack_free = 0;
-					uint8_t *stack_sweeper = (uint8_t *)system_load.tasks[i].tcb->stack_alloc_ptr;
+				while (stack_free < stack_size) {
+					if (*stack_sweeper++ != 0xff)
+						break;
 
-					while (stack_free < stack_size) {
-						if (*stack_sweeper++ != 0xff)
-							break;
+					stack_free++;
+				}
 
-						stack_free++;
-					}
+				printf(CL "%4d %*-s %8lld %2d.%03d %5u/%5u %3u (%3u) ",
+					   system_load.tasks[i].tcb->pid,
+					   CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
+					   (system_load.tasks[i].total_runtime / 1000),
+					   (int)(curr_loads[i] * 100),
+					   (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
+					   stack_size - stack_free,
+					   stack_size,
+					   system_load.tasks[i].tcb->sched_priority,
+					   system_load.tasks[i].tcb->base_priority);
 
-					printf("\033[K % 2d\t%s%s % 8lld ms%s  \t % 2d.%03d \t % 4u / % 4u",
-					       (int)system_load.tasks[i].tcb->pid,
-					       system_load.tasks[i].tcb->name,
-					       spaces,
-					       (system_load.tasks[i].total_runtime / 1000),
-					       runtime_spaces,
-					       (int)(curr_loads[i] * 100),
-					       (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
-					       stack_size - stack_free,
-					       stack_size);
-					/* Print scheduling info with RR time slice */
 #if CONFIG_RR_INTERVAL > 0
-					printf("\t%d\t(%d)\t\t%d\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority, (int)system_load.tasks[i].tcb->timeslice);
+				/* print scheduling info with RR time slice */
+				printf(" %6d\n", system_load.tasks[i].tcb->timeslice);
 #else
-					/* Print scheduling info without time slice*/
-					printf("\t%d (%d)\n", (int)system_load.tasks[i].tcb->sched_priority, (int)system_load.tasks[i].tcb->base_priority);
+				// print task state instead
+				printf(" %-6s\n", tstate_name(system_load.tasks[i].tcb->task_state));
 #endif
-				}
 			}
 		}
 
-		printf("\033[K[ Hit Ctrl-C to quit. ]\n\033[J");
-		fflush(stdout);
-
 		interval_start_time = new_time;
 
-		char c;
-
-		/* Sleep 200 ms waiting for user input four times */
+		/* Sleep 200 ms waiting for user input five times ~ 1s */
 		/* XXX use poll ... */
-		for (int k = 0; k < 4; k++) {
+		for (int k = 0; k < 5; k++) {
+			char c;
+
 			if (read(console, &c, 1) == 1) {
-				if (c == 0x03 || c == 0x63) {
-					printf("Abort\n");
+				switch (c) {
+				case 0x03: // ctrl-c
+				case 0x1b: // esc
+				case 'c':
+				case 'q':
 					close(console);
 					return OK;
+					/* not reached */
 				}
 			}