Merge remote-tracking branch 'upstream/master' into hott-esc

8 files changed, 550 insertions, 571 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/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'");
 }