Merge branch 'master' into seatbelt_multirotor

40 files changed, 1174 insertions, 793 deletions

diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c
index ecd31a073..be8968a4e 100644
--- a/src/drivers/ardrone_interface/ardrone_motor_control.c
+++ b/src/drivers/ardrone_interface/ardrone_motor_control.c
@@ -109,7 +109,7 @@ int ar_multiplexing_init()
 {
 	int		fd;
 	
-	fd = open(GPIO_DEVICE_PATH, 0);
+	fd = open(PX4FMU_DEVICE_PATH, 0);
 
 	if (fd < 0) {
 		warn("GPIO: open fail");
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 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/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index b54b7aba1..e78d96569 100644
--- a/src/drivers/mkblctrl/mkblctrl.cpp
+++ b/src/drivers/mkblctrl/mkblctrl.cpp
@@ -74,6 +74,7 @@
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/esc_status.h>
 
 #include <systemlib/err.h>
 
@@ -87,7 +88,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
 {
@@ -119,6 +120,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);
@@ -136,11 +138,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;
@@ -214,6 +220,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
@@ -243,8 +250,10 @@ MK::MK(int bus) :
 	_t_armed(-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),
@@ -464,6 +473,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)
 {
@@ -506,8 +522,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));
@@ -515,6 +529,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;
@@ -602,9 +622,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 */
@@ -612,7 +634,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
 					}
 
 				}
@@ -635,8 +660,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_armed);
@@ -715,17 +775,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)
 {
@@ -738,17 +798,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;
@@ -1014,8 +1072,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;
 		}
@@ -1112,25 +1169,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;
@@ -1282,7 +1333,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;
@@ -1341,6 +1392,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 {
@@ -1406,6 +1460,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;
 
@@ -1461,11 +1516,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);
 	}
 
@@ -1483,7 +1548,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 5147ac500..a98b527b1 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -166,7 +166,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 19163cebe..ae56b70b3 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -129,6 +129,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;
@@ -173,6 +183,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[]);
@@ -313,7 +329,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),
@@ -338,7 +354,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;
@@ -700,8 +717,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;
@@ -1271,13 +1286,14 @@ PX4IO::print_status()
 	printf("%u bytes free\n",
 		io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM));
 	uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS);
-	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n",
+	printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n",
 		flags,
 		((flags & PX4IO_P_STATUS_FLAGS_ARMED)    ? " ARMED" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_OK)    ? " RC_OK" : " RC_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_PPM)   ? " PPM" : ""),
-		((flags & PX4IO_P_STATUS_FLAGS_RC_DSM)   ? " DSM" : ""),
+		(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""),
+		(((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS)  ? " SBUS" : ""),
 		((flags & PX4IO_P_STATUS_FLAGS_FMU_OK)   ? " FMU_OK" : " FMU_FAIL"),
 		((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)  ? " RAW_PPM" : ""),
@@ -1372,7 +1388,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;
 
@@ -1424,6 +1441,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 */
@@ -1466,18 +1503,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:
@@ -1614,10 +1664,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;
@@ -1626,7 +1731,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");
@@ -1800,7 +1905,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");
 		}
@@ -1844,7 +1949,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);
@@ -1918,6 +2023,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' or 'update'");
+	errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'");
 }
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index 27888523b..d13ffe414 100644
--- a/src/examples/fixedwing_control/main.c
+++ b/src/examples/fixedwing_control/main.c
@@ -512,7 +512,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 c177c8fd2..c96f73155 100644
--- a/src/examples/flow_position_control/flow_position_control_main.c
+++ b/src/examples/flow_position_control/flow_position_control_main.c
@@ -101,7 +101,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[])
 {
@@ -118,7 +118,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 c0b16d2e7..e40c9081d 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -90,7 +90,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[])
 {
@@ -107,7 +107,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 9648728c8..8b3881c43 100644
--- a/src/examples/flow_speed_control/flow_speed_control_main.c
+++ b/src/examples/flow_speed_control/flow_speed_control_main.c
@@ -99,7 +99,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[])
 {
@@ -116,7 +116,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/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c
index 43cbe74c7..1aef739c7 100644
--- a/src/modules/gpio_led/gpio_led.c
+++ b/src/modules/gpio_led/gpio_led.c
@@ -53,10 +53,12 @@
 #include <uORB/topics/vehicle_status.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;
@@ -75,51 +77,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]);
@@ -131,15 +179,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 */
@@ -150,11 +209,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)
@@ -200,7 +259,6 @@ void gpio_led_cycle(FAR void *arg)
 
 		if (led_state_new) {
 			ioctl(priv->gpio_fd, GPIO_SET, priv->pin);
-
 		} else {
 			ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin);
 		}
@@ -211,7 +269,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 5b8345e7e..7c1c4b175 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>
@@ -471,7 +470,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));
 }
@@ -479,7 +478,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];
@@ -488,7 +487,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 0597555ab..edb8761b8 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..405046750 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"
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 cb6d6b16a..20fb11b2c 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 0acbea675..68d49c24b 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 674f9dddd..88d8cc87c 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -105,6 +105,7 @@
 #define PX4IO_P_STATUS_FLAGS_ARM_SYNC		(1 << 9) /* the arming state between IO and FMU is in sync */
 #define PX4IO_P_STATUS_FLAGS_INIT_OK		(1 << 10) /* initialisation of the IO completed without error */
 #define PX4IO_P_STATUS_FLAGS_FAILSAFE		(1 << 11) /* failsafe is active */
+#define PX4IO_P_STATUS_FLAGS_RC_DSM11		(1 << 12) /* DSM input is 11 bit data */
 
 #define PX4IO_P_STATUS_ALARMS			3	 /* alarm flags - alarms latch, write 1 to a bit to clear it */
 #define PX4IO_P_STATUS_ALARMS_VBATT_LOW		(1 << 0) /* VBatt is very close to regulator dropout */
@@ -156,7 +157,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 272cdb7bf..83feeb9b6 100644
--- a/src/modules/px4iofirmware/px4io.h
+++ b/src/modules/px4iofirmware/px4io.h
@@ -184,6 +184,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 df7d6dcd3..a922362b6 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -349,10 +349,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:
@@ -360,6 +360,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 b90b3e178..1eb2c7b98 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -79,6 +79,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,7 +615,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	/* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */
 	/* number of messages */
-	const ssize_t fdsc = 17;
+	const ssize_t fdsc = 18;
 	/* Sanity check variable and index */
 	ssize_t fdsc_count = 0;
 	/* file descriptors to wait for */
@@ -642,6 +643,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));
 
@@ -663,6 +665,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 		int flow_sub;
 		int rc_sub;
 		int airspeed_sub;
+		int esc_sub;
 	} subs;
 
 	/* log message buffer: header + body */
@@ -686,6 +689,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_ARSP_s log_ARSP;
 			struct log_FLOW_s log_FLOW;
 			struct log_GPOS_s log_GPOS;
+			struct log_ESC_s log_ESC;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -795,6 +799,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.
@@ -1105,6 +1115,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 1b2237d65..d9f1cd634 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -209,6 +209,24 @@ struct log_GPOS_s {
 	float vel_e;
 	float vel_d;
 };
+
+/* --- ESC - ESC STATE --- */
+#define LOG_ESC_MSG 64
+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 */
@@ -230,6 +248,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"),
 	LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"),
 	LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"),
+	LOG_FORMAT(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 f6f4d60c7..252c1b7a9 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -155,6 +155,7 @@ PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
 
 PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */
+PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 1ded14a91..ae5a55109 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -230,6 +230,8 @@ private:
 		float rc_scale_flaps;
 
 		float battery_voltage_scaling;
+
+		int   rc_rl1_DSM_VCC_control;
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -278,6 +280,8 @@ private:
 		param_t rc_scale_flaps;
 
 		param_t battery_voltage_scaling;
+
+		param_t rc_rl1_DSM_VCC_control;
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -514,6 +518,9 @@ Sensors::Sensors() :
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
+	/* DSM VCC relay control */
+	_parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -730,6 +737,11 @@ Sensors::parameters_update()
 		warnx("Failed updating voltage scaling param");
 	}
 
+	/* relay 1 DSM VCC control */
+	if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) {
+		warnx("Failed updating relay 1 DSM VCC control");
+	}
+
 	return OK;
 }
 
diff --git a/src/modules/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 e7d7e7bca..ae5fc6c61 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -169,3 +169,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..e67a39e1e
--- /dev/null
+++ b/src/modules/uORB/topics/esc_status.h
@@ -0,0 +1,114 @@
+/****************************************************************************
+ *
+ *   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 */
+};
+
+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/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 */
 				}
 			}