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

110 files changed, 2048 insertions, 537 deletions

diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c
index aeaf830de..735bdb41a 100644
--- a/src/drivers/ardrone_interface/ardrone_interface.c
+++ b/src/drivers/ardrone_interface/ardrone_interface.c
@@ -114,7 +114,7 @@ int ardrone_interface_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		ardrone_interface_task = task_spawn("ardrone_interface",
+		ardrone_interface_task = task_spawn_cmd("ardrone_interface",
 						    SCHED_DEFAULT,
 						    SCHED_PRIORITY_MAX - 15,
 						    2048,
diff --git a/src/drivers/boards/px4fmu/px4fmu_init.c b/src/drivers/boards/px4fmu/px4fmu_init.c
index 69edc23ab..212a92cfa 100644
--- a/src/drivers/boards/px4fmu/px4fmu_init.c
+++ b/src/drivers/boards/px4fmu/px4fmu_init.c
@@ -58,7 +58,7 @@
 #include <nuttx/mmcsd.h>
 #include <nuttx/analog/adc.h>
 
-#include "stm32_internal.h"
+#include "stm32.h"
 #include "px4fmu_internal.h"
 #include "stm32_uart.h"
 
diff --git a/src/drivers/boards/px4fmu/px4fmu_internal.h b/src/drivers/boards/px4fmu/px4fmu_internal.h
index 671a58f8f..383a046ff 100644
--- a/src/drivers/boards/px4fmu/px4fmu_internal.h
+++ b/src/drivers/boards/px4fmu/px4fmu_internal.h
@@ -50,7 +50,7 @@
 __BEGIN_DECLS
 
 /* these headers are not C++ safe */
-#include <stm32_internal.h>
+#include <stm32.h>
 
  
 /****************************************************************************************************
diff --git a/src/drivers/boards/px4fmu/px4fmu_led.c b/src/drivers/boards/px4fmu/px4fmu_led.c
index 34fd194c3..31b25984e 100644
--- a/src/drivers/boards/px4fmu/px4fmu_led.c
+++ b/src/drivers/boards/px4fmu/px4fmu_led.c
@@ -41,7 +41,7 @@
 
 #include <stdbool.h>
 
-#include "stm32_internal.h"
+#include "stm32.h"
 #include "px4fmu_internal.h"
 
 #include <arch/board/board.h>
diff --git a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
index cb8918306..d85131dd8 100644
--- a/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
+++ b/src/drivers/boards/px4fmu/px4fmu_pwm_servo.c
@@ -46,7 +46,7 @@
 #include <arch/board/board.h>
 #include <drivers/drv_pwm_output.h>
 
-#include <stm32_internal.h>
+#include <stm32.h>
 #include <stm32_gpio.h>
 #include <stm32_tim.h>
 
diff --git a/src/drivers/boards/px4fmu/px4fmu_spi.c b/src/drivers/boards/px4fmu/px4fmu_spi.c
index b5d00eac0..e05ddecf3 100644
--- a/src/drivers/boards/px4fmu/px4fmu_spi.c
+++ b/src/drivers/boards/px4fmu/px4fmu_spi.c
@@ -52,7 +52,7 @@
 
 #include "up_arch.h"
 #include "chip.h"
-#include "stm32_internal.h"
+#include "stm32.h"
 #include "px4fmu_internal.h"
 
 /************************************************************************************
diff --git a/src/drivers/boards/px4fmu/px4fmu_usb.c b/src/drivers/boards/px4fmu/px4fmu_usb.c
index b0b669fbe..0be981c1e 100644
--- a/src/drivers/boards/px4fmu/px4fmu_usb.c
+++ b/src/drivers/boards/px4fmu/px4fmu_usb.c
@@ -52,7 +52,7 @@
 #include <nuttx/usb/usbdev_trace.h>
 
 #include "up_arch.h"
-#include "stm32_internal.h"
+#include "stm32.h"
 #include "px4fmu_internal.h"
 
 /************************************************************************************
diff --git a/src/drivers/boards/px4io/px4io_init.c b/src/drivers/boards/px4io/px4io_init.c
index d36353c6f..15c59e423 100644
--- a/src/drivers/boards/px4io/px4io_init.c
+++ b/src/drivers/boards/px4io/px4io_init.c
@@ -54,7 +54,7 @@
 
 #include <nuttx/arch.h>
 
-#include "stm32_internal.h"
+#include "stm32.h"
 #include "px4io_internal.h"
 #include "stm32_uart.h"
 
diff --git a/src/drivers/boards/px4io/px4io_internal.h b/src/drivers/boards/px4io/px4io_internal.h
index eb2820bb7..6638e715e 100644
--- a/src/drivers/boards/px4io/px4io_internal.h
+++ b/src/drivers/boards/px4io/px4io_internal.h
@@ -47,7 +47,7 @@
 #include <nuttx/compiler.h>
 #include <stdint.h>
 
-#include <stm32_internal.h>
+#include <stm32.h>
 
 /************************************************************************************
  * Definitions
diff --git a/src/drivers/boards/px4io/px4io_pwm_servo.c b/src/drivers/boards/px4io/px4io_pwm_servo.c
index a2f73c429..6df470da6 100644
--- a/src/drivers/boards/px4io/px4io_pwm_servo.c
+++ b/src/drivers/boards/px4io/px4io_pwm_servo.c
@@ -46,7 +46,7 @@
 #include <arch/board/board.h>
 #include <drivers/drv_pwm_output.h>
 
-#include <stm32_internal.h>
+#include <stm32.h>
 #include <stm32_gpio.h>
 #include <stm32_tim.h>
 
diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h
index 92d184326..7e3998fca 100644
--- a/src/drivers/drv_gpio.h
+++ b/src/drivers/drv_gpio.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -42,7 +42,7 @@
 
 #include <sys/ioctl.h>
 
-#ifdef CONFIG_ARCH_BOARD_PX4FMU
+#ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 /*
  * PX4FMU GPIO numbers.
  *
@@ -67,7 +67,7 @@
 
 #endif
 
-#ifdef CONFIG_ARCH_BOARD_PX4IO
+#ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 /*
  * PX4IO GPIO numbers.
  *
diff --git a/src/drivers/ets_airspeed/ets_airspeed.cpp b/src/drivers/ets_airspeed/ets_airspeed.cpp
index c39da98d7..b34d3fa5d 100644
--- a/src/drivers/ets_airspeed/ets_airspeed.cpp
+++ b/src/drivers/ets_airspeed/ets_airspeed.cpp
@@ -84,8 +84,9 @@
 
 /**
  * The Eagle Tree Airspeed V3 cannot provide accurate reading below speeds of 15km/h.
+ * You can set this value to 12 if you want a zero reading below 15km/h.
  */
-#define MIN_ACCURATE_DIFF_PRES_PA 12
+#define MIN_ACCURATE_DIFF_PRES_PA 0
 
 /* Measurement rate is 100Hz */
 #define CONVERSION_INTERVAL	(1000000 / 100)	/* microseconds */
@@ -463,8 +464,8 @@ ETSAirspeed::collect()
 
 	uint16_t diff_pres_pa = val[1] << 8 | val[0];
 
+	// XXX move the parameter read out of the driver.
 	param_get(param_find("SENS_DPRES_OFF"), &_diff_pres_offset);
-
 	if (diff_pres_pa < _diff_pres_offset + MIN_ACCURATE_DIFF_PRES_PA) {
 		diff_pres_pa = 0;
 
diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp
index d9aa772d4..bd027ce0b 100644
--- a/src/drivers/hil/hil.cpp
+++ b/src/drivers/hil/hil.cpp
@@ -224,7 +224,7 @@ HIL::init()
 	// gpio_reset();
 
 	/* start the HIL interface task */
-	_task = task_spawn("fmuhil",
+	_task = task_spawn_cmd("fmuhil",
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
 			   2048,
diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.c b/src/drivers/hott/hott_telemetry/hott_telemetry.c
index c87810b42..a88f357f6 100644
--- a/src/drivers/hott/hott_telemetry/hott_telemetry.c
+++ b/src/drivers/hott/hott_telemetry/hott_telemetry.c
@@ -255,7 +255,7 @@ hott_telemetry_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn(daemon_name,
+		deamon_task = task_spawn_cmd(daemon_name,
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 40,
 					 2048,
diff --git a/src/drivers/hott/messages.c b/src/drivers/hott/messages.c
index ba2f6212d..36d5fe942 100644
--- a/src/drivers/hott/messages.c
+++ b/src/drivers/hott/messages.c
@@ -159,7 +159,7 @@ build_eam_response(uint8_t *buffer, size_t *size)
 	memset(&airspeed, 0, sizeof(airspeed));
 	orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed);
 
-	uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s);
+	uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f);
 	msg.speed_L = (uint8_t)speed & 0xff;
 	msg.speed_H = (uint8_t)(speed >> 8) & 0xff;
 
diff --git a/src/drivers/md25/md25_main.cpp b/src/drivers/md25/md25_main.cpp
index 80850e708..e62c46b0d 100644
--- a/src/drivers/md25/md25_main.cpp
+++ b/src/drivers/md25/md25_main.cpp
@@ -109,7 +109,7 @@ int md25_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn("md25",
+		deamon_task = task_spawn_cmd("md25",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 10,
 					 2048,
diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp
index c67276f8a..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),
@@ -313,7 +322,7 @@ MK::init(unsigned motors)
 	gpio_reset();
 
 	/* start the IO interface task */
-	_task = task_spawn("mkblctrl",
+	_task = task_spawn_cmd("mkblctrl",
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_MAX - 20,
 			   2048,
@@ -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/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index bf72892eb..5147ac500 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -239,7 +239,7 @@ PX4FMU::init()
 	gpio_reset();
 
 	/* start the IO interface task */
-	_task = task_spawn("fmuservo",
+	_task = task_spawn_cmd("fmuservo",
 			   SCHED_DEFAULT,
 			   SCHED_PRIORITY_DEFAULT,
 			   2048,
diff --git a/src/drivers/px4io/uploader.cpp b/src/drivers/px4io/uploader.cpp
index 15524c3ae..9e3f041e3 100644
--- a/src/drivers/px4io/uploader.cpp
+++ b/src/drivers/px4io/uploader.cpp
@@ -49,6 +49,7 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <poll.h>
+#include <termios.h>
 #include <sys/stat.h>
 
 #include "uploader.h"
@@ -121,6 +122,12 @@ PX4IO_Uploader::upload(const char *filenames[])
 		return -errno;
 	}
 
+	/* adjust line speed to match bootloader */
+	struct termios t;
+	tcgetattr(_io_fd, &t);
+	cfsetspeed(&t, 115200);
+	tcsetattr(_io_fd, TCSANOW, &t);
+
 	/* look for the bootloader */
 	ret = sync();
 
@@ -251,7 +258,7 @@ PX4IO_Uploader::recv(uint8_t &c, unsigned timeout)
 	int ret = ::poll(&fds[0], 1, timeout);
 
 	if (ret < 1) {
-		log("poll timeout %d", ret);
+		//log("poll timeout %d", ret);
 		return -ETIMEDOUT;
 	}
 
diff --git a/src/drivers/stm32/adc/adc.cpp b/src/drivers/stm32/adc/adc.cpp
index 911def943..1020eb946 100644
--- a/src/drivers/stm32/adc/adc.cpp
+++ b/src/drivers/stm32/adc/adc.cpp
@@ -58,7 +58,7 @@
 #include <drivers/drv_adc.h>
 
 #include <arch/stm32/chip.h>
-#include <stm32_internal.h>
+#include <stm32.h>
 #include <stm32_gpio.h>
 
 #include <systemlib/err.h>
diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c
index fd63681e3..7ef3db970 100644
--- a/src/drivers/stm32/drv_hrt.c
+++ b/src/drivers/stm32/drv_hrt.c
@@ -66,7 +66,7 @@
 #include "up_internal.h"
 #include "up_arch.h"
 
-#include "stm32_internal.h"
+#include "stm32.h"
 #include "stm32_gpio.h"
 #include "stm32_tim.h"
 
@@ -278,8 +278,10 @@ static void		hrt_call_invoke(void);
 #ifdef CONFIG_HRT_PPM
 /* 
  * If the timer hardware doesn't support GTIM_CCER_CCxNP, then we will work around it.
+ *
+ * Note that we assume that M3 means STM32F1 (since we don't really care about the F2).
  */
-# ifndef GTIM_CCER_CC1NP
+# ifdef CONFIG_ARCH_CORTEXM3
 #  define GTIM_CCER_CC1NP 0
 #  define GTIM_CCER_CC2NP 0
 #  define GTIM_CCER_CC3NP 0
diff --git a/src/drivers/stm32/drv_pwm_servo.c b/src/drivers/stm32/drv_pwm_servo.c
index c1efb8515..7b060412c 100644
--- a/src/drivers/stm32/drv_pwm_servo.c
+++ b/src/drivers/stm32/drv_pwm_servo.c
@@ -64,7 +64,7 @@
 #include <up_internal.h>
 #include <up_arch.h>
 
-#include <stm32_internal.h>
+#include <stm32.h>
 #include <stm32_gpio.h>
 #include <stm32_tim.h>
 
diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
index ac5511e60..167ef30a8 100644
--- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp
+++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp
@@ -111,7 +111,7 @@
 #include <up_internal.h>
 #include <up_arch.h>
 
-#include <stm32_internal.h>
+#include <stm32.h>
 #include <stm32_gpio.h>
 #include <stm32_tim.h>
 
diff --git a/src/examples/fixedwing_control/main.c b/src/examples/fixedwing_control/main.c
index 89fbef020..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[])
 {
@@ -528,7 +528,7 @@ int ex_fixedwing_control_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn("ex_fixedwing_control",
+		deamon_task = task_spawn_cmd("ex_fixedwing_control",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 20,
 					 2048,
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_control/flow_position_control_params.c b/src/examples/flow_position_control/flow_position_control_params.c
index 4f3598a57..eb1473647 100644
--- a/src/examples/flow_position_control/flow_position_control_params.c
+++ b/src/examples/flow_position_control/flow_position_control_params.c
@@ -40,14 +40,25 @@
 #include "flow_position_control_params.h"
 
 /* controller parameters */
+
+// Position control P gain
 PARAM_DEFINE_FLOAT(FPC_POS_P, 3.0f);
+// Position control D / damping gain
 PARAM_DEFINE_FLOAT(FPC_POS_D, 0.0f);
+// Altitude control P gain
 PARAM_DEFINE_FLOAT(FPC_H_P, 0.15f);
+// Altitude control I (integrator) gain
 PARAM_DEFINE_FLOAT(FPC_H_I, 0.00001f);
+// Altitude control D gain
 PARAM_DEFINE_FLOAT(FPC_H_D, 0.8f);
+// Altitude control rate limiter
 PARAM_DEFINE_FLOAT(FPC_H_RATE, 0.1f);
+// Altitude control minimum altitude
 PARAM_DEFINE_FLOAT(FPC_H_MIN, 0.5f);
+// Altitude control maximum altitude (higher than 1.5m is untested)
 PARAM_DEFINE_FLOAT(FPC_H_MAX, 1.5f);
+// Altitude control feed forward throttle - adjust to the
+// throttle position (0..1) where the copter hovers in manual flight
 PARAM_DEFINE_FLOAT(FPC_T_FFWD, 0.7f); // adjust this before flight
 PARAM_DEFINE_FLOAT(FPC_L_S_X, 1.2f);
 PARAM_DEFINE_FLOAT(FPC_L_S_Y, 1.2f);
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/examples/px4_daemon_app/px4_daemon_app.c b/src/examples/px4_daemon_app/px4_daemon_app.c
index c568aaadc..53f1b4a9a 100644
--- a/src/examples/px4_daemon_app/px4_daemon_app.c
+++ b/src/examples/px4_daemon_app/px4_daemon_app.c
@@ -95,7 +95,7 @@ int px4_daemon_app_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		daemon_task = task_spawn("daemon",
+		daemon_task = task_spawn_cmd("daemon",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT,
 					 4096,
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index c3836bdfa..97d7fdd75 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -40,6 +40,7 @@
 #include <poll.h>
 
 #include "KalmanNav.hpp"
+#include <systemlib/err.h>
 
 // constants
 // Titterton pg. 52
@@ -58,14 +59,17 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	P0(9, 9),
 	V(6, 6),
 	// attitude measurement ekf matrices
-	HAtt(6, 9),
-	RAtt(6, 6),
+	HAtt(4, 9),
+	RAtt(4, 4),
 	// position measurement ekf matrices
 	HPos(6, 9),
 	RPos(6, 6),
 	// attitude representations
 	C_nb(),
 	q(),
+	_accel_sub(-1),
+	_gyro_sub(-1),
+	_mag_sub(-1),
 	// subscriptions
 	_sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz
 	_gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz
@@ -123,8 +127,19 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	lon = 0.0f;
 	alt = 0.0f;
 
-	// initialize quaternions
-	q = Quaternion(EulerAngles(phi, theta, psi));
+	// gyro, accel and mag subscriptions
+	_gyro_sub = orb_subscribe(ORB_ID(sensor_gyro));
+	_accel_sub = orb_subscribe(ORB_ID(sensor_accel));
+	_mag_sub = orb_subscribe(ORB_ID(sensor_mag));
+
+	struct accel_report accel;
+	orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel);
+
+	struct mag_report mag;
+	orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag);
+
+	// initialize rotation quaternion with a single raw sensor measurement
+	q = init(accel.x, accel.y, accel.z, mag.x, mag.y, mag.z);
 
 	// initialize dcm
 	C_nb = Dcm(q);
@@ -141,6 +156,45 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	updateParams();
 }
 
+math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz)
+{
+    float initialRoll, initialPitch;
+    float cosRoll, sinRoll, cosPitch, sinPitch;
+    float magX, magY;
+    float initialHdg, cosHeading, sinHeading;
+
+    initialRoll = atan2(-ay, -az);
+    initialPitch = atan2(ax, -az);
+
+    cosRoll = cosf(initialRoll);
+    sinRoll = sinf(initialRoll);
+    cosPitch = cosf(initialPitch);
+    sinPitch = sinf(initialPitch);
+
+    magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch;
+
+    magY = my * cosRoll - mz * sinRoll;
+
+    initialHdg = atan2f(-magY, magX);
+
+    cosRoll = cosf(initialRoll * 0.5f);
+    sinRoll = sinf(initialRoll * 0.5f);
+
+    cosPitch = cosf(initialPitch * 0.5f);
+    sinPitch = sinf(initialPitch * 0.5f);
+
+    cosHeading = cosf(initialHdg * 0.5f);
+    sinHeading = sinf(initialHdg * 0.5f);
+
+    float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading;
+    float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading;
+    float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading;
+    float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading;
+
+    return math::Quaternion(q0, q1, q2, q3);
+
+}
+
 void KalmanNav::update()
 {
 	using namespace math;
@@ -181,8 +235,8 @@ void KalmanNav::update()
 		if (correctAtt() == ret_ok) _attitudeInitCounter++;
 
 		if (_attitudeInitCounter > 100) {
-			printf("[kalman_demo] initialized EKF attitude\n");
-			printf("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
+			warnx("initialized EKF attitude\n");
+			warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f\n",
 			       double(phi), double(theta), double(psi));
 			_attitudeInitialized = true;
 		}
@@ -202,13 +256,13 @@ void KalmanNav::update()
 		setLonDegE7(_gps.lon);
 		setAltE3(_gps.alt);
 		_positionInitialized = true;
-		printf("[kalman_demo] initialized EKF state with GPS\n");
-		printf("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+		warnx("initialized EKF state with GPS\n");
+		warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
 		       double(vN), double(vE), double(vD),
 		       lat, lon, alt);
 	}
 
-	// prediciton step
+	// prediction step
 	// using sensors timestamp so we can account for packet lag
 	float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f;
 	//printf("dt: %15.10f\n", double(dt));
@@ -233,7 +287,7 @@ void KalmanNav::update()
 	// attitude correction step
 	if (_attitudeInitialized 								// initialized
 	    && sensorsUpdate 								// new data
-	    && _sensors.timestamp - _attTimeStamp > 1e6 / 20 	// 20 Hz
+	    && _sensors.timestamp - _attTimeStamp > 1e6 / 50 	// 50 Hz
 	   ) {
 		_attTimeStamp = _sensors.timestamp;
 		correctAtt();
@@ -480,26 +534,32 @@ int KalmanNav::correctAtt()
 	// trig
 	float cosPhi = cosf(phi);
 	float cosTheta = cosf(theta);
-	float cosPsi = cosf(psi);
+	// float cosPsi = cosf(psi);
 	float sinPhi = sinf(phi);
 	float sinTheta = sinf(theta);
-	float sinPsi = sinf(psi);
-
-	// mag measurement
-	Vector3 zMag(_sensors.magnetometer_ga);
-	//float magNorm = zMag.norm();
-	zMag = zMag.unit();
+	// float sinPsi = sinf(psi);
 
 	// mag predicted measurement
 	// choosing some typical magnetic field properties,
 	//  TODO dip/dec depend on lat/ lon/ time
-	float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
+	//float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level
 	float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north
-	float bN = cosf(dip) * cosf(dec);
-	float bE = cosf(dip) * sinf(dec);
-	float bD = sinf(dip);
-	Vector3 bNav(bN, bE, bD);
-	Vector3 zMagHat = (C_nb.transpose() * bNav).unit();
+
+	// compensate roll and pitch, but not yaw
+	// XXX take the vectors out of the C_nb matrix to avoid singularities
+	math::Dcm C_rp(math::EulerAngles(phi, theta, 0.0f));//C_nb.transpose();
+
+	// mag measurement
+	Vector3 magBody(_sensors.magnetometer_ga);
+
+	// transform to earth frame
+	Vector3 magNav = C_rp * magBody;
+
+	// calculate error between estimate and measurement
+	// apply declination correction for true heading as well.
+	float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec;
+	if (yMag > M_PI_F) yMag -= 2*M_PI_F;
+	if (yMag < -M_PI_F) yMag += 2*M_PI_F;
 
 	// accel measurement
 	Vector3 zAccel(_sensors.accelerometer_m_s2);
@@ -512,9 +572,9 @@ int KalmanNav::correctAtt()
 	bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f;
 
 	if (ignoreAccel) {
+		RAttAdjust(1, 1) = 1.0e10;
+		RAttAdjust(2, 2) = 1.0e10;
 		RAttAdjust(3, 3) = 1.0e10;
-		RAttAdjust(4, 4) = 1.0e10;
-		RAttAdjust(5, 5) = 1.0e10;
 
 	} else {
 		//printf("correcting attitude with accel\n");
@@ -523,58 +583,25 @@ int KalmanNav::correctAtt()
 	// accel predicted measurement
 	Vector3 zAccelHat = (C_nb.transpose() * Vector3(0, 0, -_g.get())).unit();
 
-	// combined measurement
-	Vector zAtt(6);
-	Vector zAttHat(6);
+	// calculate residual
+	Vector y(4);
+	y(0) = yMag;
+	y(1) = zAccel(0) - zAccelHat(0);
+	y(2) = zAccel(1) - zAccelHat(1);
+	y(3) = zAccel(2) - zAccelHat(2);
 
-	for (int i = 0; i < 3; i++) {
-		zAtt(i) = zMag(i);
-		zAtt(i + 3) = zAccel(i);
-		zAttHat(i) = zMagHat(i);
-		zAttHat(i + 3) = zAccelHat(i);
-	}
+	// HMag
+	HAtt(0, 2) = 1;
 
-	// HMag , HAtt (0-2,:)
-	float tmp1 =
-		cosPsi * cosTheta * bN +
-		sinPsi * cosTheta * bE -
-		sinTheta * bD;
-	HAtt(0, 1) = -(
-			     cosPsi * sinTheta * bN +
-			     sinPsi * sinTheta * bE +
-			     cosTheta * bD
-		     );
-	HAtt(0, 2) = -cosTheta * (sinPsi * bN - cosPsi * bE);
-	HAtt(1, 0) =
-		(cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bN +
-		(cosPhi * sinPsi * sinTheta - sinPhi * cosPsi) * bE +
-		cosPhi * cosTheta * bD;
-	HAtt(1, 1) = sinPhi * tmp1;
-	HAtt(1, 2) = -(
-			     (sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bN -
-			     (sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bE
-		     );
-	HAtt(2, 0) = -(
-			     (sinPhi * cosPsi * sinTheta - cosPhi * sinPsi) * bN +
-			     (sinPhi * sinPsi * sinTheta + cosPhi * cosPsi) * bE +
-			     (sinPhi * cosTheta) * bD
-		     );
-	HAtt(2, 1) = cosPhi * tmp1;
-	HAtt(2, 2) = -(
-			     (cosPhi * sinPsi * sinTheta - sinPhi * cosTheta) * bN -
-			     (cosPhi * cosPsi * sinTheta + sinPhi * sinPsi) * bE
-		     );
-
-	// HAccel , HAtt (3-5,:)
-	HAtt(3, 1) = cosTheta;
-	HAtt(4, 0) = -cosPhi * cosTheta;
-	HAtt(4, 1) = sinPhi * sinTheta;
-	HAtt(5, 0) = sinPhi * cosTheta;
-	HAtt(5, 1) = cosPhi * sinTheta;
+	// HAccel
+	HAtt(1, 1) = cosTheta;
+	HAtt(2, 0) = -cosPhi * cosTheta;
+	HAtt(2, 1) = sinPhi * sinTheta;
+	HAtt(3, 0) = sinPhi * cosTheta;
+	HAtt(3, 1) = cosPhi * sinTheta;
 
 	// compute correction
 	// http://en.wikipedia.org/wiki/Extended_Kalman_filter
-	Vector y = zAtt - zAttHat; // residual
 	Matrix S = HAtt * P * HAtt.transpose() + RAttAdjust; // residual covariance
 	Matrix K = P * HAtt.transpose() * S.inverse();
 	Vector xCorrect = K * y;
@@ -585,7 +612,7 @@ int KalmanNav::correctAtt()
 
 		if (isnan(val) || isinf(val)) {
 			// abort correction and return
-			printf("[kalman_demo] numerical failure in att correction\n");
+			warnx("numerical failure in att correction\n");
 			// reset P matrix to P0
 			P = P0;
 			return ret_error;
@@ -615,11 +642,8 @@ int KalmanNav::correctAtt()
 	float beta = y.dot(S.inverse() * y);
 
 	if (beta > _faultAtt.get()) {
-		printf("fault in attitude: beta = %8.4f\n", (double)beta);
-		printf("y:\n"); y.print();
-		printf("zMagHat:\n"); zMagHat.print();
-		printf("zMag:\n"); zMag.print();
-		printf("bNav:\n"); bNav.print();
+		warnx("fault in attitude: beta = %8.4f", (double)beta);
+		warnx("y:\n"); y.print();
 	}
 
 	// update quaternions from euler
@@ -652,9 +676,9 @@ int KalmanNav::correctPos()
 	for (size_t i = 0; i < xCorrect.getRows(); i++) {
 		float val = xCorrect(i);
 
-		if (isnan(val) || isinf(val)) {
+		if (!isfinite(val)) {
 			// abort correction and return
-			printf("[kalman_demo] numerical failure in gps correction\n");
+			warnx("numerical failure in gps correction\n");
 			// fallback to GPS
 			vN = _gps.vel_n_m_s;
 			vE = _gps.vel_e_m_s;
@@ -685,8 +709,8 @@ int KalmanNav::correctPos()
 
 	static int counter = 0;
 	if (beta > _faultPos.get() && (counter % 10 == 0)) {
-		printf("fault in gps: beta = %8.4f\n", (double)beta);
-		printf("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f\n",
+		warnx("fault in gps: beta = %8.4f", (double)beta);
+		warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f",
 		       double(y(0) / sqrtf(RPos(0, 0))),
 		       double(y(1) / sqrtf(RPos(1, 1))),
 		       double(y(2) / sqrtf(RPos(2, 2))),
@@ -722,8 +746,6 @@ void KalmanNav::updateParams()
 	if (noiseMagSq < noiseMin) noiseMagSq = noiseMin;
 
 	RAtt(0, 0) = noiseMagSq; // normalized direction
-	RAtt(1, 1) = noiseMagSq;
-	RAtt(2, 2) = noiseMagSq;
 
 	// accelerometer noise
 	float noiseAccelSq = _rAccel.get() * _rAccel.get();
@@ -731,9 +753,9 @@ void KalmanNav::updateParams()
 	// bound noise to prevent singularities
 	if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin;
 
-	RAtt(3, 3) = noiseAccelSq; // normalized direction
-	RAtt(4, 4) = noiseAccelSq;
-	RAtt(5, 5) = noiseAccelSq;
+	RAtt(1, 1) = noiseAccelSq; // normalized direction
+	RAtt(2, 2) = noiseAccelSq;
+	RAtt(3, 3) = noiseAccelSq;
 
 	// gps noise
 	float R = R0 + float(alt);
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index c2bf18115..49d0d157d 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -56,6 +56,10 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/parameter_update.h>
 
+#include <drivers/drv_accel.h>
+#include <drivers/drv_gyro.h>
+#include <drivers/drv_mag.h>
+
 #include <drivers/drv_hrt.h>
 #include <poll.h>
 #include <unistd.h>
@@ -78,6 +82,9 @@ public:
 	 */
 
 	virtual ~KalmanNav() {};
+
+	math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz);
+
 	/**
 	 * The main callback function for the class
 	 */
@@ -136,6 +143,11 @@ protected:
 	// publications
 	control::UOrbPublication<vehicle_global_position_s> _pos;       /**< position pub. */
 	control::UOrbPublication<vehicle_attitude_s> _att;              /**< attitude pub. */
+
+	int			_accel_sub;		/**< Accelerometer subscription */
+	int			_gyro_sub;		/**< Gyroscope subscription */
+	int			_mag_sub;		/**< Magnetometer subscription */
+
 	// time stamps
 	uint64_t _pubTimeStamp;     /**< output data publication time stamp */
 	uint64_t _predictTimeStamp; /**< prediction time stamp */
@@ -151,7 +163,8 @@ protected:
 	enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT};  /**< state enumeration */
 	float phi, theta, psi;                  /**< 3-2-1 euler angles */
 	float vN, vE, vD;                       /**< navigation velocity, m/s */
-	double lat, lon, alt;                   /**< lat, lon, alt, radians */
+	double lat, lon;                   	/**< lat, lon radians */
+	float alt;                   		/**< altitude, meters */
 	// parameters
 	control::BlockParam<float> _vGyro;      /**< gyro process noise */
 	control::BlockParam<float> _vAccel;     /**< accelerometer process noise  */
diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
index 10592ec7c..4befdc879 100644
--- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp
+++ b/src/modules/att_pos_estimator_ekf/kalman_main.cpp
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Example User <mail@example.com>
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: 	James Goppert
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -33,8 +33,10 @@
  ****************************************************************************/
 
 /**
- * @file kalman_demo.cpp
- * Demonstration of control library
+ * @file kalman_main.cpp
+ * Combined attitude / position estimator.
+ *
+ * @author James Goppert
  */
 
 #include <nuttx/config.h>
@@ -51,7 +53,7 @@
 
 static bool thread_should_exit = false;     /**< Deamon exit flag */
 static bool thread_running = false;     /**< Deamon status flag */
-static int deamon_task;             /**< Handle of deamon task / thread */
+static int daemon_task;             /**< Handle of deamon task / thread */
 
 /**
  * Deamon management function.
@@ -101,9 +103,10 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn("att_pos_estimator_ekf",
+
+		daemon_task = task_spawn_cmd("att_pos_estimator_ekf",
 					 SCHED_DEFAULT,
-					 SCHED_PRIORITY_MAX - 5,
+					 SCHED_PRIORITY_MAX - 30,
 					 4096,
 					 kalman_demo_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
@@ -133,7 +136,7 @@ int att_pos_estimator_ekf_main(int argc, char *argv[])
 int kalman_demo_thread_main(int argc, char *argv[])
 {
 
-	warnx("starting\n");
+	warnx("starting");
 
 	using namespace math;
 
@@ -145,7 +148,7 @@ int kalman_demo_thread_main(int argc, char *argv[])
 		nav.update();
 	}
 
-	printf("exiting.\n");
+	warnx("exiting.");
 
 	thread_running = false;
 
diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk
index 21b7c9166..8d4a40d95 100644
--- a/src/modules/att_pos_estimator_ekf/module.mk
+++ b/src/modules/att_pos_estimator_ekf/module.mk
@@ -37,9 +37,6 @@
 
 MODULE_COMMAND	= att_pos_estimator_ekf
 
-# XXX this might be intended for the spawned deamon, validate
-MODULE_PRIORITY	 = "SCHED_PRIORITY_MAX-30"
-
 SRCS		= kalman_main.cpp \
 		  KalmanNav.cpp \
 		  params.c
diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c
index 50642f067..4af5edead 100644
--- a/src/modules/att_pos_estimator_ekf/params.c
+++ b/src/modules/att_pos_estimator_ekf/params.c
@@ -1,12 +1,45 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
 #include <systemlib/param/param.h>
 
 /*PARAM_DEFINE_FLOAT(NAME,0.0f);*/
 PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f);
 PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_MAG, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 1.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 5.0f);
-PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 5.0f);
+PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f);
+PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f);
 PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f);
 PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f);
 PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f);
diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 16d5ad626..d8b40ac3b 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -123,7 +123,7 @@ int attitude_estimator_ekf_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		attitude_estimator_ekf_task = task_spawn("attitude_estimator_ekf",
+		attitude_estimator_ekf_task = task_spawn_cmd("attitude_estimator_ekf",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
 					      14000,
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
index 3cbc62ea1..3ca50fb39 100755
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
@@ -117,7 +117,7 @@ int attitude_estimator_so3_comp_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		attitude_estimator_so3_comp_task = task_spawn("attitude_estimator_so3_comp",
+		attitude_estimator_so3_comp_task = task_spawn_cmd("attitude_estimator_so3_comp",
 					      SCHED_DEFAULT,
 					      SCHED_PRIORITY_MAX - 5,
 					      12400,
diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c
index d79dd93dd..48a36ac26 100644
--- a/src/modules/commander/accelerometer_calibration.c
+++ b/src/modules/commander/accelerometer_calibration.c
@@ -1,12 +1,45 @@
-/*
- * accelerometer_calibration.c
+/****************************************************************************
+ *
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 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
+ * are met:
  *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ * 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.
  *
- * Transform acceleration vector to true orientation and scale
+ * 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 accelerometer_calibration.c
  *
- * * * * Model * * *
+ * Implementation of accelerometer calibration.
+ *
+ * Transform acceleration vector to true orientation, scale and offset
+ *
+ * ===== Model =====
  * accel_corr = accel_T * (accel_raw - accel_offs)
  *
  * accel_corr[3] - fully corrected acceleration vector in body frame
@@ -14,7 +47,7 @@
  * accel_raw[3]  - raw acceleration vector
  * accel_offs[3] - acceleration offset vector
  *
- * * * * Calibration * * *
+ * ===== Calibration =====
  *
  * Reference vectors
  * accel_corr_ref[6][3] = [  g  0  0 ]     // nose up
@@ -34,7 +67,6 @@
  *
  * accel_offs[i] = (accel_raw_ref[i*2][i] + accel_raw_ref[i*2+1][i]) / 2
  *
- *
  * Find accel_T
  *
  * 9 unknown constants
@@ -67,6 +99,8 @@
  *
  * accel_T = A^-1 * g
  * g = 9.80665
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include "accelerometer_calibration.h"
diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h
index a11cf93d3..f93a867ba 100644
--- a/src/modules/commander/accelerometer_calibration.h
+++ b/src/modules/commander/accelerometer_calibration.h
@@ -1,8 +1,43 @@
-/*
- * accelerometer_calibration.h
+/****************************************************************************
  *
- *   Copyright (C) 2013 Anton Babushkin. All rights reserved.
- *   Author: 	Anton Babushkin	<rk3dov@gmail.com>
+ *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Author: 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
+ * 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 accelerometer_calibration.h
+ *
+ * Definition of accelerometer calibration.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef ACCELEROMETER_CALIBRATION_H_
diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c
index aab8f3e04..67f053e22 100644
--- a/src/modules/commander/commander.c
+++ b/src/modules/commander/commander.c
@@ -1200,7 +1200,7 @@ int commander_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		daemon_task = task_spawn("commander",
+		daemon_task = task_spawn_cmd("commander",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 40,
 					 3000,
diff --git a/src/modules/controllib/block/UOrbPublication.hpp b/src/modules/controllib/block/UOrbPublication.hpp
index a36f4429f..0a8ae2ff7 100644
--- a/src/modules/controllib/block/UOrbPublication.hpp
+++ b/src/modules/controllib/block/UOrbPublication.hpp
@@ -60,11 +60,15 @@ public:
 		List<UOrbPublicationBase *> * list,
 		const struct orb_metadata *meta) :
 		_meta(meta),
-		_handle() {
+		_handle(-1) {
 		if (list != NULL) list->add(this);
 	}
 	void update() {
-		orb_publish(getMeta(), getHandle(), getDataVoidPtr());
+		if (_handle > 0) {
+			orb_publish(getMeta(), getHandle(), getDataVoidPtr());
+		} else {
+			setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
+		}
 	}
 	virtual void *getDataVoidPtr() = 0;
 	virtual ~UOrbPublicationBase() {
@@ -99,10 +103,6 @@ public:
 		const struct orb_metadata *meta) :
 		T(), // initialize data structure to zero
 		UOrbPublicationBase(list, meta) {
-		// It is important that we call T()
-		// before we publish the data, so we
-		// call this here instead of the base class
-		setHandle(orb_advertise(getMeta(), getDataVoidPtr()));
 	}
 	virtual ~UOrbPublication() {}
 	/*
diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp
index 7be38015c..77b2ac806 100644
--- a/src/modules/controllib/fixedwing.cpp
+++ b/src/modules/controllib/fixedwing.cpp
@@ -130,7 +130,7 @@ BlockUorbEnabledAutopilot::BlockUorbEnabledAutopilot(SuperBlock *parent, const c
 	_attCmd(&getSubscriptions(), ORB_ID(vehicle_attitude_setpoint), 20),
 	_ratesCmd(&getSubscriptions(), ORB_ID(vehicle_rates_setpoint), 20),
 	_pos(&getSubscriptions() , ORB_ID(vehicle_global_position), 20),
-	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_setpoint), 20),
+	_posCmd(&getSubscriptions(), ORB_ID(vehicle_global_position_set_triplet), 20),
 	_manual(&getSubscriptions(), ORB_ID(manual_control_setpoint), 20),
 	_status(&getSubscriptions(), ORB_ID(vehicle_status), 20),
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
@@ -213,7 +213,7 @@ void BlockMultiModeBacksideAutopilot::update()
 	// only update guidance in auto mode
 	if (_status.state_machine == SYSTEM_STATE_AUTO) {
 		// update guidance
-		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
+		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
 	}
 
 	// XXX handle STABILIZED (loiter on spot) as well
@@ -225,7 +225,7 @@ void BlockMultiModeBacksideAutopilot::update()
 	    _status.state_machine == SYSTEM_STATE_STABILIZED) {
 
 		// update guidance
-		_guide.update(_pos, _att, _posCmd, _lastPosCmd);
+		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
 
 		// calculate velocity, XXX should be airspeed, but using ground speed for now
 		// for the purpose of control we will limit the velocity feedback between
@@ -239,7 +239,7 @@ void BlockMultiModeBacksideAutopilot::update()
 		float vCmd = _vLimit.update(_vCmd.get());
 
 		// altitude hold
-		float dThrottle = _h2Thr.update(_posCmd.altitude - _pos.alt);
+		float dThrottle = _h2Thr.update(_posCmd.current.altitude - _pos.alt);
 
 		// heading hold
 		float psiError = _wrap_pi(_guide.getPsiCmd() - _att.yaw);
@@ -328,7 +328,7 @@ void BlockMultiModeBacksideAutopilot::update()
 			_actuators.control[CH_ELV] = _stabilization.getElevator() + _trimElv.get();
 			_actuators.control[CH_RDR] = _stabilization.getRudder() + _trimRdr.get();
 
-			// currenlty using manual throttle
+			// currently using manual throttle
 			// XXX if you enable this watch out, vz might be very noisy
 			//_actuators.control[CH_THR] = dThrottle + _trimThr.get();
 			_actuators.control[CH_THR] = _manual.throttle;
diff --git a/src/modules/controllib/fixedwing.hpp b/src/modules/controllib/fixedwing.hpp
index 53d0cf893..e4028c40d 100644
--- a/src/modules/controllib/fixedwing.hpp
+++ b/src/modules/controllib/fixedwing.hpp
@@ -43,7 +43,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/vehicle_global_position.h>
-#include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_global_position_set_triplet.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/vehicle_status.h>
 #include <uORB/topics/actuator_controls.h>
@@ -280,7 +280,7 @@ protected:
 	UOrbSubscription<vehicle_attitude_setpoint_s> _attCmd;
 	UOrbSubscription<vehicle_rates_setpoint_s> _ratesCmd;
 	UOrbSubscription<vehicle_global_position_s> _pos;
-	UOrbSubscription<vehicle_global_position_setpoint_s> _posCmd;
+	UOrbSubscription<vehicle_global_position_set_triplet_s> _posCmd;
 	UOrbSubscription<manual_control_setpoint_s> _manual;
 	UOrbSubscription<vehicle_status_s> _status;
 	UOrbSubscription<parameter_update_s> _param_update;
@@ -328,7 +328,7 @@ private:
 	BlockParam<float> _crMax;
 
 	struct pollfd _attPoll;
-	vehicle_global_position_setpoint_s _lastPosCmd;
+	vehicle_global_position_set_triplet_s _lastPosCmd;
 	enum {CH_AIL, CH_ELV, CH_RDR, CH_THR};
 	uint64_t _timeStamp;
 public:
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
index 769b8b0a8..2aeca3a98 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c
@@ -128,8 +128,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att
 	if (!initialized) {
 		parameters_init(&h);
 		parameters_update(&h, &p);
-		pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
-		pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller
+		pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
+		pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE, 0.0f); //P Controller
 		initialized = true;
 	}
 
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
index 58477632b..6c9c137bb 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_main.c
@@ -336,7 +336,7 @@ int fixedwing_att_control_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn("fixedwing_att_control",
+		deamon_task = task_spawn_cmd("fixedwing_att_control",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 20,
 					 2048,
diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
index 4eccc118c..cdab39edc 100644
--- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
+++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c
@@ -179,9 +179,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 	if (!initialized) {
 		parameters_init(&h);
 		parameters_update(&h, &p);
-		pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller
-		pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
-		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
+		pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the controller layout is with a PI rate controller
+		pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
+		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE, 0.0f); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher
 		initialized = true;
 	}
 
diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
index c3d57a85a..4803a526e 100644
--- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
+++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp
@@ -108,7 +108,8 @@ int fixedwing_backside_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn("fixedwing_backside",
+
+		deamon_task = task_spawn_cmd("fixedwing_backside",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 10,
 					 5120,
diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
index 71c78f5b8..73df3fb9e 100644
--- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
+++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c
@@ -239,10 +239,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[])
 
 	parameters_init(&h);
 	parameters_update(&h, &p);
-	pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit
-	pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE);
-	pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE);
-	pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value
+	pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE, 0.0f); //arbitrary high limit
+	pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
+	pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE, 0.0f);
+	pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE, 0.0f); //TODO: remove hardcoded value
 
 	/* error and performance monitoring */
 	perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval");
@@ -448,7 +448,7 @@ int fixedwing_pos_control_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn("fixedwing_pos_control",
+		deamon_task = task_spawn_cmd("fixedwing_pos_control",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 20,
 					 2048,
diff --git a/src/modules/mathlib/math/Dcm.cpp b/src/modules/mathlib/math/Dcm.cpp
index c3742e288..f509f7081 100644
--- a/src/modules/mathlib/math/Dcm.cpp
+++ b/src/modules/mathlib/math/Dcm.cpp
@@ -69,6 +69,15 @@ Dcm::Dcm(float c00, float c01, float c02,
 	dcm(2, 2) = c22;
 }
 
+Dcm::Dcm(const float data[3][3]) :
+	Matrix(3, 3)
+{
+	Dcm &dcm = *this;
+	/* set rotation matrix */
+	for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+		dcm(i, j) = data[i][j];
+}
+
 Dcm::Dcm(const float *data) :
 	Matrix(3, 3, data)
 {
diff --git a/src/modules/mathlib/math/Dcm.hpp b/src/modules/mathlib/math/Dcm.hpp
index 28d840b10..df8970d3a 100644
--- a/src/modules/mathlib/math/Dcm.hpp
+++ b/src/modules/mathlib/math/Dcm.hpp
@@ -77,6 +77,11 @@ public:
 	Dcm(const float *data);
 
 	/**
+	 * array ctor
+	 */
+	Dcm(const float data[3][3]);
+
+	/**
 	 * quaternion ctor
 	 */
 	Dcm(const Quaternion &q);
diff --git a/src/modules/mathlib/math/Limits.cpp b/src/modules/mathlib/math/Limits.cpp
new file mode 100644
index 000000000..d4c892d8a
--- /dev/null
+++ b/src/modules/mathlib/math/Limits.cpp
@@ -0,0 +1,146 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Limits.cpp
+ *
+ * Limiting / constrain helper functions
+ */
+
+
+#include <math.h>
+#include <stdint.h>
+
+#include "Limits.hpp"
+
+
+namespace math {
+
+
+float __EXPORT min(float val1, float val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+int __EXPORT min(int val1, int val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+unsigned __EXPORT min(unsigned val1, unsigned val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+uint64_t __EXPORT min(uint64_t val1, uint64_t val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+double __EXPORT min(double val1, double val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+float __EXPORT max(float val1, float val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+int __EXPORT max(int val1, int val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+unsigned __EXPORT max(unsigned val1, unsigned val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+uint64_t __EXPORT max(uint64_t val1, uint64_t val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+double __EXPORT max(double val1, double val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
+
+float __EXPORT constrain(float val, float min, float max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+int __EXPORT constrain(int val, int min, int max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+double __EXPORT constrain(double val, double min, double max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
+float __EXPORT radians(float degrees)
+{
+	return (degrees / 180.0f) * M_PI_F;
+}
+
+double __EXPORT radians(double degrees)
+{
+	return (degrees / 180.0) * M_PI;
+}
+
+float __EXPORT degrees(float radians)
+{
+	return (radians / M_PI_F) * 180.0f;
+}
+
+double __EXPORT degrees(double radians)
+{
+	return (radians / M_PI) * 180.0;
+}
+
+}
\ No newline at end of file
diff --git a/src/modules/mathlib/math/Limits.hpp b/src/modules/mathlib/math/Limits.hpp
new file mode 100644
index 000000000..fb778dd66
--- /dev/null
+++ b/src/modules/mathlib/math/Limits.hpp
@@ -0,0 +1,87 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Limits.hpp
+ *
+ * Limiting / constrain helper functions
+ */
+
+#pragma once
+
+#include <nuttx/config.h>
+#include <stdint.h>
+
+namespace math {
+
+
+float __EXPORT min(float val1, float val2);
+
+int __EXPORT min(int val1, int val2);
+
+unsigned __EXPORT min(unsigned val1, unsigned val2);
+
+uint64_t __EXPORT min(uint64_t val1, uint64_t val2);
+
+double __EXPORT min(double val1, double val2);
+
+float __EXPORT max(float val1, float val2);
+
+int __EXPORT max(int val1, int val2);
+
+unsigned __EXPORT max(unsigned val1, unsigned val2);
+
+uint64_t __EXPORT max(uint64_t val1, uint64_t val2);
+
+double __EXPORT max(double val1, double val2);
+
+
+float __EXPORT constrain(float val, float min, float max);
+
+int __EXPORT constrain(int val, int min, int max);
+
+unsigned __EXPORT constrain(unsigned val, unsigned min, unsigned max);
+
+uint64_t __EXPORT constrain(uint64_t val, uint64_t min, uint64_t max);
+
+double __EXPORT constrain(double val, double min, double max);
+
+float __EXPORT radians(float degrees);
+
+double __EXPORT radians(double degrees);
+
+float __EXPORT degrees(float radians);
+
+double __EXPORT degrees(double radians);
+
+}
\ No newline at end of file
diff --git a/src/modules/mathlib/math/Quaternion.hpp b/src/modules/mathlib/math/Quaternion.hpp
index 4b4e959d8..048a55d33 100644
--- a/src/modules/mathlib/math/Quaternion.hpp
+++ b/src/modules/mathlib/math/Quaternion.hpp
@@ -37,7 +37,7 @@
  * math quaternion lib
  */
 
-//#pragma once
+#pragma once
 
 #include "Vector.hpp"
 #include "Matrix.hpp"
diff --git a/src/modules/mathlib/math/Vector2f.cpp b/src/modules/mathlib/math/Vector2f.cpp
new file mode 100644
index 000000000..68e741817
--- /dev/null
+++ b/src/modules/mathlib/math/Vector2f.cpp
@@ -0,0 +1,103 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector2f.cpp
+ *
+ * math vector
+ */
+
+#include "test/test.hpp"
+
+#include "Vector2f.hpp"
+
+namespace math
+{
+
+Vector2f::Vector2f() :
+	Vector(2)
+{
+}
+
+Vector2f::Vector2f(const Vector &right) :
+	Vector(right)
+{
+#ifdef VECTOR_ASSERT
+	ASSERT(right.getRows() == 2);
+#endif
+}
+
+Vector2f::Vector2f(float x, float y) :
+	Vector(2)
+{
+	setX(x);
+	setY(y);
+}
+
+Vector2f::Vector2f(const float *data) :
+	Vector(2, data)
+{
+}
+
+Vector2f::~Vector2f()
+{
+}
+
+float Vector2f::cross(const Vector2f &b) const
+{
+	const Vector2f &a = *this;
+	return a(0)*b(1) - a(1)*b(0);
+}
+
+float Vector2f::operator %(const Vector2f &v) const
+{
+	return cross(v);
+}
+    
+float Vector2f::operator *(const Vector2f &v) const
+{
+    return dot(v);
+}
+
+int __EXPORT vector2fTest()
+{
+	printf("Test Vector2f\t\t: ");
+	// test float ctor
+	Vector2f v(1, 2);
+	ASSERT(equal(v(0), 1));
+	ASSERT(equal(v(1), 2));
+	printf("PASS\n");
+	return 0;
+}
+
+} // namespace math
diff --git a/src/modules/mathlib/math/Vector2f.hpp b/src/modules/mathlib/math/Vector2f.hpp
new file mode 100644
index 000000000..ecd62e81c
--- /dev/null
+++ b/src/modules/mathlib/math/Vector2f.hpp
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file Vector2f.hpp
+ *
+ * math 3 vector
+ */
+
+#pragma once
+
+#include "Vector.hpp"
+
+namespace math
+{
+
+class __EXPORT Vector2f :
+	public Vector
+{
+public:
+	Vector2f();
+	Vector2f(const Vector &right);
+	Vector2f(float x, float y);
+	Vector2f(const float *data);
+	virtual ~Vector2f();
+	float cross(const Vector2f &b) const;
+	float operator %(const Vector2f &v) const;
+    float operator *(const Vector2f &v) const;
+    inline Vector2f operator*(const float &right) const {
+		return Vector::operator*(right);
+	}
+
+	/**
+	 * accessors
+	 */
+	void setX(float x) { (*this)(0) = x; }
+	void setY(float y) { (*this)(1) = y; }
+	const float &getX() const { return (*this)(0); }
+	const float &getY() const { return (*this)(1); }
+};
+    
+class __EXPORT Vector2 :
+	public Vector2f
+{
+};
+
+int __EXPORT vector2fTest();
+} // math
+
diff --git a/src/modules/mathlib/math/Vector3.cpp b/src/modules/mathlib/math/Vector3.cpp
index 61fcc442f..dcb85600e 100644
--- a/src/modules/mathlib/math/Vector3.cpp
+++ b/src/modules/mathlib/math/Vector3.cpp
@@ -74,9 +74,9 @@ Vector3::~Vector3()
 {
 }
 
-Vector3 Vector3::cross(const Vector3 &b)
+Vector3 Vector3::cross(const Vector3 &b) const
 {
-	Vector3 &a = *this;
+	const Vector3 &a = *this;
 	Vector3 result;
 	result(0) = a(1) * b(2) - a(2) * b(1);
 	result(1) = a(2) * b(0) - a(0) * b(2);
diff --git a/src/modules/mathlib/math/Vector3.hpp b/src/modules/mathlib/math/Vector3.hpp
index 8c36ac134..568d9669a 100644
--- a/src/modules/mathlib/math/Vector3.hpp
+++ b/src/modules/mathlib/math/Vector3.hpp
@@ -53,7 +53,7 @@ public:
 	Vector3(float x, float y, float z);
 	Vector3(const float *data);
 	virtual ~Vector3();
-	Vector3 cross(const Vector3 &b);
+	Vector3 cross(const Vector3 &b) const;
 
 	/**
 	 * accessors
@@ -65,6 +65,11 @@ public:
 	const float &getY() const { return (*this)(1); }
 	const float &getZ() const { return (*this)(2); }
 };
+    
+class __EXPORT Vector3f :
+	public Vector3
+{
+};
 
 int __EXPORT vector3Test();
 } // math
diff --git a/src/modules/mathlib/math/arm/Vector.hpp b/src/modules/mathlib/math/arm/Vector.hpp
index 58d51107d..4155800e8 100644
--- a/src/modules/mathlib/math/arm/Vector.hpp
+++ b/src/modules/mathlib/math/arm/Vector.hpp
@@ -178,8 +178,15 @@ public:
 			    getRows());
 		return result;
 	}
+	inline Vector operator-(void) const {
+		Vector result(getRows());
+		arm_negate_f32((float *)getData(),
+			    result.getData(),
+			    getRows());
+		return result;
+	}
 	// other functions
-	inline float dot(const Vector &right) {
+	inline float dot(const Vector &right) const {
 		float result = 0;
 		arm_dot_prod_f32((float *)getData(),
 				 (float *)right.getData(),
@@ -187,12 +194,21 @@ public:
 				 &result);
 		return result;
 	}
-	inline float norm() {
+	inline float norm() const {
 		return sqrtf(dot(*this));
 	}
-	inline Vector unit() {
+	inline float length() const {
+		return norm();
+	}
+	inline Vector unit() const {
 		return (*this) / norm();
 	}
+	inline Vector normalized() const {
+		return unit();
+	}
+	inline void normalize() {
+		(*this) = (*this) / norm();
+	}
 	inline static Vector zero(size_t rows) {
 		Vector result(rows);
 		// calloc returns zeroed memory
diff --git a/src/modules/mathlib/math/generic/Vector.hpp b/src/modules/mathlib/math/generic/Vector.hpp
index 1a7363779..8cfdc676d 100644
--- a/src/modules/mathlib/math/generic/Vector.hpp
+++ b/src/modules/mathlib/math/generic/Vector.hpp
@@ -184,8 +184,17 @@ public:
 
 		return result;
 	}
+	inline Vector operator-(void) const {
+		Vector result(getRows());
+
+		for (size_t i = 0; i < getRows(); i++) {
+			result(i) = -((*this)(i));
+		}
+
+		return result;
+	}
 	// other functions
-	inline float dot(const Vector &right) {
+	inline float dot(const Vector &right) const {
 		float result = 0;
 
 		for (size_t i = 0; i < getRows(); i++) {
@@ -194,12 +203,21 @@ public:
 
 		return result;
 	}
-	inline float norm() {
+	inline float norm() const {
 		return sqrtf(dot(*this));
 	}
-	inline Vector unit() {
+	inline float length() const {
+		return norm();
+	}
+	inline Vector unit() const {
 		return (*this) / norm();
 	}
+	inline Vector normalized() const {
+		return unit();
+	}
+	inline void normalize() {
+		(*this) = (*this) / norm();
+	}
 	inline static Vector zero(size_t rows) {
 		Vector result(rows);
 		// calloc returns zeroed memory
diff --git a/src/modules/mathlib/mathlib.h b/src/modules/mathlib/mathlib.h
index b919d53db..40ffb22bc 100644
--- a/src/modules/mathlib/mathlib.h
+++ b/src/modules/mathlib/mathlib.h
@@ -39,12 +39,16 @@
 
 #ifdef __cplusplus
 
+#pragma once
+
 #include "math/Dcm.hpp"
 #include "math/EulerAngles.hpp"
 #include "math/Matrix.hpp"
 #include "math/Quaternion.hpp"
 #include "math/Vector.hpp"
 #include "math/Vector3.hpp"
+#include "math/Vector2f.hpp"
+#include "math/Limits.hpp"
 
 #endif
 
diff --git a/src/modules/mathlib/module.mk b/src/modules/mathlib/module.mk
index bcdb2afe5..2146a1413 100644
--- a/src/modules/mathlib/module.mk
+++ b/src/modules/mathlib/module.mk
@@ -36,11 +36,13 @@
 #
 SRCS		 = math/test/test.cpp \
 		   math/Vector.cpp \
+		   math/Vector2f.cpp \
 		   math/Vector3.cpp \
 		   math/EulerAngles.cpp \
 		   math/Quaternion.cpp \
 		   math/Dcm.cpp \
-		   math/Matrix.cpp
+		   math/Matrix.cpp \
+		   math/Limits.cpp
 
 #
 # In order to include .config we first have to save off the
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index de78cd139..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>
@@ -144,14 +143,6 @@ set_hil_on_off(bool hil_enabled)
 	/* Enable HIL */
 	if (hil_enabled && !mavlink_hil_enabled) {
 
-		/* Advertise topics */
-		pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
-		pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
-
-		/* sensore level hil */
-		pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
-		pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
-
 		mavlink_hil_enabled = true;
 
 		/* ramp up some HIL-related subscriptions */
@@ -479,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));
 }
@@ -487,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];
@@ -496,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];
@@ -714,6 +705,8 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		lowspeed_counter++;
 
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+
 		/* sleep quarter the time */
 		usleep(25000);
 
@@ -725,10 +718,13 @@ int mavlink_thread_main(int argc, char *argv[])
 
 		/* send parameters at 20 Hz (if queued for sending) */
 		mavlink_pm_queued_send();
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
 
 		/* sleep quarter the time */
 		usleep(25000);
 
+		mavlink_waypoint_eventloop(mavlink_missionlib_get_system_timestamp(), &global_pos, &local_pos);
+
 		if (baudrate > 57600) {
 			mavlink_pm_queued_send();
 		}
@@ -787,7 +783,7 @@ int mavlink_main(int argc, char *argv[])
 			errx(0, "mavlink already running\n");
 
 		thread_should_exit = false;
-		mavlink_task = task_spawn("mavlink",
+		mavlink_task = task_spawn_cmd("mavlink",
 					  SCHED_DEFAULT,
 					  SCHED_PRIORITY_DEFAULT,
 					  2048,
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_hil.h b/src/modules/mavlink/mavlink_hil.h
index 8c7a5b514..744ed7d94 100644
--- a/src/modules/mavlink/mavlink_hil.h
+++ b/src/modules/mavlink/mavlink_hil.h
@@ -41,15 +41,6 @@
 
 extern bool mavlink_hil_enabled;
 
-extern struct vehicle_global_position_s hil_global_pos;
-extern struct vehicle_attitude_s hil_attitude;
-extern struct sensor_combined_s hil_sensors;
-extern struct vehicle_gps_position_s hil_gps;
-extern orb_advert_t pub_hil_global_pos;
-extern orb_advert_t pub_hil_attitude;
-extern orb_advert_t pub_hil_sensors;
-extern orb_advert_t pub_hil_gps;
-
 /**
  * Enable / disable Hardware in the Loop simulation mode.
  *
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.c b/src/modules/mavlink/mavlink_receiver.cpp
index 940d030b2..01bbabd46 100644
--- a/src/modules/mavlink/mavlink_receiver.c
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -49,8 +49,6 @@
 #include <fcntl.h>
 #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>
@@ -62,10 +60,17 @@
 #include <stdlib.h>
 #include <poll.h>
 
+#include <mathlib/mathlib.h>
+
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
+#include <systemlib/err.h>
+#include <systemlib/airspeed.h>
 #include <mavlink/mavlink_log.h>
 
+__BEGIN_DECLS
+
+#include "mavlink_bridge_header.h"
 #include "waypoints.h"
 #include "orb_topics.h"
 #include "missionlib.h"
@@ -73,8 +78,12 @@
 #include "mavlink_parameters.h"
 #include "util.h"
 
+extern bool gcs_link;
+
+__END_DECLS
+
 /* XXX should be in a header somewhere */
-pthread_t receive_start(int uart);
+extern "C" pthread_t receive_start(int uart);
 
 static void handle_message(mavlink_message_t *msg);
 static void *receive_thread(void *arg);
@@ -88,18 +97,18 @@ struct vehicle_global_position_s hil_global_pos;
 struct vehicle_attitude_s hil_attitude;
 struct vehicle_gps_position_s hil_gps;
 struct sensor_combined_s hil_sensors;
-orb_advert_t pub_hil_global_pos = -1;
-orb_advert_t pub_hil_attitude = -1;
-orb_advert_t pub_hil_gps = -1;
-orb_advert_t pub_hil_sensors = -1;
+static orb_advert_t pub_hil_global_pos = -1;
+static orb_advert_t pub_hil_attitude = -1;
+static orb_advert_t pub_hil_gps = -1;
+static orb_advert_t pub_hil_sensors = -1;
+static orb_advert_t pub_hil_airspeed = -1;
 
 static orb_advert_t cmd_pub = -1;
 static orb_advert_t flow_pub = -1;
 
 static orb_advert_t offboard_control_sp_pub = -1;
 static orb_advert_t vicon_position_pub = -1;
-
-extern bool gcs_link;
+static orb_advert_t telemetry_status_pub = -1;
 
 static void
 handle_message(mavlink_message_t *msg)
@@ -141,10 +150,10 @@ handle_message(mavlink_message_t *msg)
 				/* check if topic is advertised */
 				if (cmd_pub <= 0) {
 					cmd_pub = orb_advertise(ORB_ID(vehicle_command), &vcmd);
+				} else {
+					/* publish */
+					orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
 				}
-
-				/* publish */
-				orb_publish(ORB_ID(vehicle_command), cmd_pub, &vcmd);
 			}
 		}
 	}
@@ -281,7 +290,7 @@ handle_message(mavlink_message_t *msg)
 			}
 
 			offboard_control_sp.armed = ml_armed;
-			offboard_control_sp.mode = ml_mode;
+			offboard_control_sp.mode = static_cast<enum OFFBOARD_CONTROL_MODE>(ml_mode);
 
 			offboard_control_sp.timestamp = hrt_absolute_time();
 
@@ -296,6 +305,33 @@ handle_message(mavlink_message_t *msg)
 		}
 	}
 
+	/* handle status updates of the radio */
+	if (msg->msgid == MAVLINK_MSG_ID_RADIO_STATUS) {
+
+		struct telemetry_status_s tstatus;
+
+		mavlink_radio_status_t rstatus;
+		mavlink_msg_radio_status_decode(msg, &rstatus);
+
+		/* publish telemetry status topic */
+		tstatus.timestamp = hrt_absolute_time();
+		tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
+		tstatus.rssi = rstatus.rssi;
+		tstatus.remote_rssi = rstatus.remrssi;
+		tstatus.txbuf = rstatus.txbuf;
+		tstatus.noise = rstatus.noise;
+		tstatus.remote_noise = rstatus.remnoise;
+		tstatus.rxerrors = rstatus.rxerrors;
+		tstatus.fixed = rstatus.fixed;
+
+		if (telemetry_status_pub == 0) {
+			telemetry_status_pub = orb_advertise(ORB_ID(telemetry_status), &tstatus);
+
+		} else {
+			orb_publish(ORB_ID(telemetry_status), telemetry_status_pub, &tstatus);
+		}
+	}
+
 	/*
 	 * Only decode hil messages in HIL mode.
 	 *
@@ -308,10 +344,10 @@ handle_message(mavlink_message_t *msg)
 
 		uint64_t timestamp = hrt_absolute_time();
 
-		if (msg->msgid == MAVLINK_MSG_ID_HIGHRES_IMU) {
+		if (msg->msgid == MAVLINK_MSG_ID_HIL_SENSOR) {
 
-			mavlink_highres_imu_t imu;
-			mavlink_msg_highres_imu_decode(msg, &imu);
+			mavlink_hil_sensor_t imu;
+			mavlink_msg_hil_sensor_decode(msg, &imu);
 
 			/* packet counter */
 			static uint16_t hil_counter = 0;
@@ -370,8 +406,34 @@ handle_message(mavlink_message_t *msg)
 			hil_sensors.magnetometer_counter = hil_counter;
 			hil_sensors.accelerometer_counter = hil_counter;
 
+			/* differential pressure */
+			hil_sensors.differential_pressure_pa = imu.diff_pressure;
+			hil_sensors.differential_pressure_counter = hil_counter;
+
+			/* airspeed from differential pressure, ambient pressure and temp */
+			struct airspeed_s airspeed;
+			airspeed.timestamp = hrt_absolute_time();
+
+			float ias = calc_indicated_airspeed(imu.diff_pressure);
+			// XXX need to fix this
+			float tas = ias;
+
+			airspeed.indicated_airspeed_m_s = ias;
+			airspeed.true_airspeed_m_s = tas;
+
+			if (pub_hil_airspeed < 0) {
+				pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
+			} else {
+				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
+			}
+			//warnx("SENSOR: IAS: %6.2f TAS: %6.2f", airspeed.indicated_airspeed_m_s, airspeed.true_airspeed_m_s);
+
 			/* publish */
-			orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+			if (pub_hil_sensors > 0) {
+				orb_publish(ORB_ID(sensor_combined), pub_hil_sensors, &hil_sensors);
+			} else {
+				pub_hil_sensors = orb_advertise(ORB_ID(sensor_combined), &hil_sensors);
+			}
 
 			// increment counters
 			hil_counter++;
@@ -379,21 +441,16 @@ handle_message(mavlink_message_t *msg)
 
 			// output
 			if ((timestamp - old_timestamp) > 10000000) {
-				printf("receiving hil imu at %d hz\n", hil_frames/10);
+				printf("receiving hil sensor at %d hz\n", hil_frames/10);
 				old_timestamp = timestamp;
 				hil_frames = 0;
 			}
 		}
 
-		if (msg->msgid == MAVLINK_MSG_ID_GPS_RAW_INT) {
+		if (msg->msgid == MAVLINK_MSG_ID_HIL_GPS) {
 
-			mavlink_gps_raw_int_t gps;
-			mavlink_msg_gps_raw_int_decode(msg, &gps);
-
-			/* packet counter */
-			static uint16_t hil_counter = 0;
-			static uint16_t hil_frames = 0;
-			static uint64_t old_timestamp = 0;
+			mavlink_hil_gps_t gps;
+			mavlink_msg_hil_gps_decode(msg, &gps);
 
 			/* gps */
 			hil_gps.timestamp_position = gps.time_usec;
@@ -412,53 +469,38 @@ handle_message(mavlink_message_t *msg)
 			/* go back to -PI..PI */
 			if (heading_rad > M_PI_F)
 				heading_rad -= 2.0f * M_PI_F;
-			hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad);
-			hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad);
-			hil_gps.vel_d_m_s = 0.0f;
+			hil_gps.vel_n_m_s = gps.vn * 1e-2f; // from cm to m
+			hil_gps.vel_e_m_s = gps.ve * 1e-2f; // from cm to m
+			hil_gps.vel_d_m_s = gps.vd * 1e-2f; // from cm to m
 			hil_gps.vel_ned_valid = true;
-			/* COG (course over ground) is speced as -PI..+PI */
+			/* COG (course over ground) is spec'ed as -PI..+PI */
 			hil_gps.cog_rad = heading_rad;
 			hil_gps.fix_type = gps.fix_type;
 			hil_gps.satellites_visible = gps.satellites_visible;
 
-			/* publish */
-			orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
-
-			// increment counters
-			hil_counter += 1 ;
-			hil_frames += 1 ;
+			/* publish GPS measurement data */
+			if (pub_hil_gps > 0) {
+				orb_publish(ORB_ID(vehicle_gps_position), pub_hil_gps, &hil_gps);
+			} else {
+				pub_hil_gps = orb_advertise(ORB_ID(vehicle_gps_position), &hil_gps);
+			}
 
-			// output
-			// if ((timestamp - old_timestamp) > 10000000) {
-			// 	printf("receiving hil gps at %d hz\n", hil_frames/10);
-			// 	old_timestamp = timestamp;
-			// 	hil_frames = 0;
-			// }
 		}
 
-		if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE) {
+		if (msg->msgid == MAVLINK_MSG_ID_HIL_STATE_QUATERNION) {
 
-			mavlink_hil_state_t hil_state;
-			mavlink_msg_hil_state_decode(msg, &hil_state);
+			mavlink_hil_state_quaternion_t hil_state;
+			mavlink_msg_hil_state_quaternion_decode(msg, &hil_state);
 
-			/* Calculate Rotation Matrix */
-			//TODO: better clarification which app does this, atm we have a ekf for quadrotors which does this, but there is no such thing if fly in fixed wing mode
-
-			if (mavlink_system.type == MAV_TYPE_FIXED_WING) {
-				//TODO: assuming low pitch and roll values for now
-				hil_attitude.R[0][0] = cosf(hil_state.yaw);
-				hil_attitude.R[0][1] = sinf(hil_state.yaw);
-				hil_attitude.R[0][2] = 0.0f;
-
-				hil_attitude.R[1][0] = -sinf(hil_state.yaw);
-				hil_attitude.R[1][1] = cosf(hil_state.yaw);
-				hil_attitude.R[1][2] = 0.0f;
-
-				hil_attitude.R[2][0] = 0.0f;
-				hil_attitude.R[2][1] = 0.0f;
-				hil_attitude.R[2][2] = 1.0f;
+			struct airspeed_s airspeed;
+			airspeed.timestamp = hrt_absolute_time();
+			airspeed.indicated_airspeed_m_s = hil_state.ind_airspeed * 1e-2f;
+			airspeed.true_airspeed_m_s = hil_state.true_airspeed * 1e-2f;
 
-				hil_attitude.R_valid = true;
+			if (pub_hil_airspeed < 0) {
+				pub_hil_airspeed = orb_advertise(ORB_ID(airspeed), &airspeed);
+			} else {
+				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
 
 			hil_global_pos.lat = hil_state.lat;
@@ -468,21 +510,48 @@ handle_message(mavlink_message_t *msg)
 			hil_global_pos.vy = hil_state.vy / 100.0f;
 			hil_global_pos.vz = hil_state.vz / 100.0f;
 
-
 			/* set timestamp and notify processes (broadcast) */
 			hil_global_pos.timestamp = hrt_absolute_time();
-			orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
 
-			hil_attitude.roll = hil_state.roll;
-			hil_attitude.pitch = hil_state.pitch;
-			hil_attitude.yaw = hil_state.yaw;
+			if (pub_hil_global_pos > 0) {
+				orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
+			} else {
+				pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
+			}
+
+			/* Calculate Rotation Matrix */
+			math::Quaternion q(hil_state.attitude_quaternion);
+			math::Dcm C_nb(q);
+			math::EulerAngles euler(C_nb);
+
+			/* set rotation matrix */
+			for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++)
+				hil_attitude.R[i][j] = C_nb(i, j);
+			
+			hil_attitude.R_valid = true;
+
+			/* set quaternion */
+			hil_attitude.q[0] = q(0);
+			hil_attitude.q[1] = q(1);
+			hil_attitude.q[2] = q(2);
+			hil_attitude.q[3] = q(3);
+			hil_attitude.q_valid = true;
+
+			hil_attitude.roll = euler.getPhi();
+			hil_attitude.pitch = euler.getTheta();
+			hil_attitude.yaw = euler.getPsi();
 			hil_attitude.rollspeed = hil_state.rollspeed;
 			hil_attitude.pitchspeed = hil_state.pitchspeed;
 			hil_attitude.yawspeed = hil_state.yawspeed;
 
 			/* set timestamp and notify processes (broadcast) */
 			hil_attitude.timestamp = hrt_absolute_time();
-			orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
+
+			if (pub_hil_attitude > 0) {
+				orb_publish(ORB_ID(vehicle_attitude), pub_hil_attitude, &hil_attitude);
+			} else {
+				pub_hil_attitude = orb_advertise(ORB_ID(vehicle_attitude), &hil_attitude);
+			}
 		}
 
 		if (msg->msgid == MAVLINK_MSG_ID_MANUAL_CONTROL) {
@@ -553,7 +622,9 @@ receive_thread(void *arg)
 
 	while (!thread_should_exit) {
 
-		struct pollfd fds[] = { { .fd = uart_fd, .events = POLLIN } };
+		struct pollfd fds[1];
+		fds[0].fd = uart_fd;
+		fds[0].events = POLLIN;
 
 		if (poll(fds, 1, timeout) > 0) {
 			/* non-blocking read. read may return negative values */
@@ -592,7 +663,7 @@ receive_start(int uart)
 	param.sched_priority = SCHED_PRIORITY_MAX - 40;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
-	pthread_attr_setstacksize(&receiveloop_attr, 2048);
+	pthread_attr_setstacksize(&receiveloop_attr, 3000);
 
 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, receive_thread, &uart);
diff --git a/src/modules/mavlink/missionlib.c b/src/modules/mavlink/missionlib.c
index d369e05ff..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>
@@ -60,6 +59,7 @@
 #include <stdlib.h>
 #include <poll.h>
 
+#include <systemlib/err.h>
 #include <systemlib/param/param.h>
 #include <systemlib/systemlib.h>
 #include <mavlink/mavlink_log.h>
@@ -73,6 +73,10 @@
 #include "mavlink_parameters.h"
 
 static uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
+static uint64_t loiter_start_time;
+
+static bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
+	struct vehicle_global_position_setpoint_s *sp);
 
 int
 mavlink_missionlib_send_message(mavlink_message_t *msg)
@@ -123,6 +127,52 @@ uint64_t mavlink_missionlib_get_system_timestamp()
 }
 
 /**
+ * Set special vehicle setpoint fields based on current mission item.
+ *
+ * @return true if the mission item could be interpreted
+ * successfully, it return false on failure.
+ */
+bool set_special_fields(float param1, float param2, float param3, float param4, uint16_t command,
+	struct vehicle_global_position_setpoint_s *sp)
+{
+	switch (command) {
+		case MAV_CMD_NAV_LOITER_UNLIM:
+			sp->nav_cmd = NAV_CMD_LOITER_UNLIMITED;
+			break;
+		case MAV_CMD_NAV_LOITER_TIME:
+			sp->nav_cmd = NAV_CMD_LOITER_TIME_LIMIT;
+			loiter_start_time = hrt_absolute_time();
+			break;
+		// case MAV_CMD_NAV_LOITER_TURNS:
+		// 	sp->nav_cmd = NAV_CMD_LOITER_TURN_COUNT;
+		// 	break;
+		case MAV_CMD_NAV_WAYPOINT:
+			sp->nav_cmd = NAV_CMD_WAYPOINT;
+			break;
+		case MAV_CMD_NAV_RETURN_TO_LAUNCH:
+			sp->nav_cmd = NAV_CMD_RETURN_TO_LAUNCH;
+			break;
+		case MAV_CMD_NAV_LAND:
+			sp->nav_cmd = NAV_CMD_LAND;
+			break;
+		case MAV_CMD_NAV_TAKEOFF:
+			sp->nav_cmd = NAV_CMD_TAKEOFF;
+			break;
+		default:
+			/* abort */
+			return false;
+	}
+
+	sp->loiter_radius = param3;
+	sp->loiter_direction = (param3 >= 0) ? 1 : -1;
+
+	sp->param1 = param1;
+	sp->param1 = param2;
+	sp->param1 = param3;
+	sp->param1 = param4;
+}
+
+/**
  * This callback is executed each time a waypoint changes.
  *
  * It publishes the vehicle_global_position_setpoint_s or the
@@ -133,9 +183,13 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		float param6_lon_y, float param7_alt_z, uint8_t frame, uint16_t command)
 {
 	static orb_advert_t global_position_setpoint_pub = -1;
+	static orb_advert_t global_position_set_triplet_pub = -1;
 	static orb_advert_t local_position_setpoint_pub = -1;
+	static unsigned last_waypoint_index = -1;
 	char buf[50] = {0};
 
+	// XXX include check if WP is supported, jump to next if not
+
 	/* Update controller setpoints */
 	if (frame == (int)MAV_FRAME_GLOBAL) {
 		/* global, absolute waypoint */
@@ -145,8 +199,9 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.altitude = param7_alt_z;
 		sp.altitude_is_relative = false;
 		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		set_special_fields(param1, param2, param3, param4, command, &sp);
 
-		/* Initialize publication if necessary */
+		/* Initialize setpoint publication if necessary */
 		if (global_position_setpoint_pub < 0) {
 			global_position_setpoint_pub = orb_advertise(ORB_ID(vehicle_global_position_setpoint), &sp);
 
@@ -154,6 +209,113 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 			orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
 		}
 
+
+		/* fill triplet: previous, current, next waypoint */
+		struct vehicle_global_position_set_triplet_s triplet;
+
+		/* current waypoint is same as sp */
+		memcpy(&(triplet.current), &sp, sizeof(sp));
+
+		/*
+		 * Check if previous WP (in mission, not in execution order) 
+		 * is available and identify correct index
+		 */
+		int last_setpoint_index = -1;
+		bool last_setpoint_valid = false;
+
+		/* at first waypoint, but cycled once through mission */
+		if (index == 0 && last_waypoint_index > 0) {
+			last_setpoint_index = last_waypoint_index;
+		} else {
+			last_setpoint_index = index - 1;
+		}
+
+		while (last_setpoint_index >= 0) {
+
+			if (wpm->waypoints[last_setpoint_index].frame == (int)MAV_FRAME_GLOBAL &&
+				(wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
+				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+				wpm->waypoints[last_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
+				last_setpoint_valid = true;
+				break;
+			}
+
+			last_setpoint_index--;
+		}
+
+		/*
+		 * Check if next WP (in mission, not in execution order) 
+		 * is available and identify correct index
+		 */
+		int next_setpoint_index = -1;
+		bool next_setpoint_valid = false;
+
+		/* at last waypoint, try to re-loop through mission as default */
+		if (index == (wpm->size - 1) && wpm->size > 1) {
+			next_setpoint_index = 0;
+		} else if (wpm->size > 1) {
+			next_setpoint_index = index + 1;
+		}
+
+		while (next_setpoint_index < wpm->size - 1) {
+
+			if (wpm->waypoints[next_setpoint_index].frame == (int)MAV_FRAME_GLOBAL && (wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_WAYPOINT ||
+					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+					wpm->waypoints[next_setpoint_index].command == (int)MAV_CMD_NAV_LOITER_UNLIM)) {
+				next_setpoint_valid = true;
+				break;
+			}
+
+			next_setpoint_index++;
+		}
+
+		/* populate last and next */
+
+		triplet.previous_valid = false;
+		triplet.next_valid = false;
+
+		if (last_setpoint_valid) {
+			triplet.previous_valid = true;
+			struct vehicle_global_position_setpoint_s sp;
+			sp.lat = wpm->waypoints[last_setpoint_index].x * 1e7f;
+			sp.lon = wpm->waypoints[last_setpoint_index].y * 1e7f;
+			sp.altitude = wpm->waypoints[last_setpoint_index].z;
+			sp.altitude_is_relative = false;
+			sp.yaw = (wpm->waypoints[last_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
+			set_special_fields(wpm->waypoints[last_setpoint_index].param1,
+				wpm->waypoints[last_setpoint_index].param2,
+				wpm->waypoints[last_setpoint_index].param3,
+				wpm->waypoints[last_setpoint_index].param4,
+				wpm->waypoints[last_setpoint_index].command, &sp);
+			memcpy(&(triplet.previous), &sp, sizeof(sp));
+		}
+
+		if (next_setpoint_valid) {
+			triplet.next_valid = true;
+			struct vehicle_global_position_setpoint_s sp;
+			sp.lat = wpm->waypoints[next_setpoint_index].x * 1e7f;
+			sp.lon = wpm->waypoints[next_setpoint_index].y * 1e7f;
+			sp.altitude = wpm->waypoints[next_setpoint_index].z;
+			sp.altitude_is_relative = false;
+			sp.yaw = (wpm->waypoints[next_setpoint_index].param4 / 180.0f) * M_PI_F - M_PI_F;
+			set_special_fields(wpm->waypoints[next_setpoint_index].param1,
+				wpm->waypoints[next_setpoint_index].param2,
+				wpm->waypoints[next_setpoint_index].param3,
+				wpm->waypoints[next_setpoint_index].param4,
+				wpm->waypoints[next_setpoint_index].command, &sp);
+			memcpy(&(triplet.next), &sp, sizeof(sp));
+		}
+
+		/* Initialize triplet publication if necessary */
+		if (global_position_set_triplet_pub < 0) {
+			global_position_set_triplet_pub = orb_advertise(ORB_ID(vehicle_global_position_set_triplet), &triplet);
+
+		} else {
+			orb_publish(ORB_ID(vehicle_global_position_set_triplet), global_position_set_triplet_pub, &triplet);
+		}
+
 		sprintf(buf, "[mp] WP#%i lat: % 3.6f/lon % 3.6f/alt % 4.6f/hdg %3.4f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
 
 	} else if (frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) {
@@ -164,6 +326,7 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		sp.altitude = param7_alt_z;
 		sp.altitude_is_relative = true;
 		sp.yaw = (param4 / 180.0f) * M_PI_F - M_PI_F;
+		set_special_fields(param1, param2, param3, param4, command, &sp);
 
 		/* Initialize publication if necessary */
 		if (global_position_setpoint_pub < 0) {
@@ -173,6 +336,8 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 			orb_publish(ORB_ID(vehicle_global_position_setpoint), global_position_setpoint_pub, &sp);
 		}
 
+
+
 		sprintf(buf, "[mp] WP#%i (lat: %f/lon %f/rel alt %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
 
 	} else if (frame == (int)MAV_FRAME_LOCAL_ENU || frame == (int)MAV_FRAME_LOCAL_NED) {
@@ -192,8 +357,15 @@ void mavlink_missionlib_current_waypoint_changed(uint16_t index, float param1,
 		}
 
 		sprintf(buf, "[mp] WP#%i (x: %f/y %f/z %f/hdg %f\n", (int)index, (double)param5_lat_x, (double)param6_lon_y, (double)param7_alt_z, (double)param4);
+	} else {
+		warnx("non-navigation WP, ignoring");
+		mavlink_missionlib_send_gcs_string("[mp] Unknown waypoint type, ignoring.");
+		return;
 	}
 
+	/* only set this for known waypoint types (non-navigation types would have returned earlier) */
+	last_waypoint_index = index;
+
 	mavlink_missionlib_send_gcs_string(buf);
 	printf("%s\n", buf);
 	//printf("[mavlink mp] new setpoint\n");//: frame: %d, lat: %d, lon: %d, alt: %d, yaw: %d\n", frame, param5_lat_x*1000, param6_lon_y*1000, param7_alt_z*1000, param4*1000);
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/module.mk b/src/modules/mavlink/module.mk
index cbf08aeb2..bfccb2d38 100644
--- a/src/modules/mavlink/module.mk
+++ b/src/modules/mavlink/module.mk
@@ -40,7 +40,7 @@ SRCS		 += mavlink.c \
 			   missionlib.c \
 			   mavlink_parameters.c \
 			   mavlink_log.c \
-			   mavlink_receiver.c \
+			   mavlink_receiver.cpp \
 			   orb_listener.c \
 			   waypoints.c
 
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index 295cd5e28..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>
@@ -72,6 +71,8 @@ struct vehicle_status_s v_status;
 struct rc_channels_s rc;
 struct rc_input_values rc_raw;
 struct actuator_armed_s armed;
+struct actuator_controls_effective_s actuators_0;
+struct vehicle_attitude_s att;
 
 struct mavlink_subscriptions mavlink_subs;
 
@@ -116,6 +117,7 @@ static void	l_debug_key_value(const struct listener *l);
 static void	l_optical_flow(const struct listener *l);
 static void	l_vehicle_rates_setpoint(const struct listener *l);
 static void	l_home(const struct listener *l);
+static void	l_airspeed(const struct listener *l);
 
 static const struct listener listeners[] = {
 	{l_sensor_combined,		&mavlink_subs.sensor_sub,	0},
@@ -140,6 +142,7 @@ static const struct listener listeners[] = {
 	{l_optical_flow,		&mavlink_subs.optical_flow,	0},
 	{l_vehicle_rates_setpoint,	&mavlink_subs.rates_setpoint_sub,	0},
 	{l_home,			&mavlink_subs.home_sub,		0},
+	{l_airspeed,			&mavlink_subs.airspeed_sub,		0},
 };
 
 static const unsigned n_listeners = sizeof(listeners) / sizeof(listeners[0]);
@@ -192,7 +195,7 @@ l_sensor_combined(const struct listener *l)
 					     raw.gyro_rad_s[1], raw.gyro_rad_s[2],
 					     raw.magnetometer_ga[0],
 					     raw.magnetometer_ga[1], raw.magnetometer_ga[2],
-					     raw.baro_pres_mbar, 0 /* no diff pressure yet */,
+					     raw.baro_pres_mbar, raw.differential_pressure_pa,
 					     raw.baro_alt_meter, raw.baro_temp_celcius,
 					     fields_updated);
 
@@ -202,9 +205,6 @@ l_sensor_combined(const struct listener *l)
 void
 l_vehicle_attitude(const struct listener *l)
 {
-	struct vehicle_attitude_s att;
-
-
 	/* copy attitude data into local buffer */
 	orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
 
@@ -564,28 +564,26 @@ l_manual_control_setpoint(const struct listener *l)
 void
 l_vehicle_attitude_controls(const struct listener *l)
 {
-	struct actuator_controls_effective_s actuators;
-
-	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators);
+	orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, mavlink_subs.actuators_sub, &actuators_0);
 
 	if (gcs_link) {
 		/* send, add spaces so that string buffer is at least 10 chars long */
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl0    ",
-						   actuators.control_effective[0]);
+						   actuators_0.control_effective[0]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl1    ",
-						   actuators.control_effective[1]);
+						   actuators_0.control_effective[1]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl2     ",
-						   actuators.control_effective[2]);
+						   actuators_0.control_effective[2]);
 		mavlink_msg_named_value_float_send(MAVLINK_COMM_0,
 						   last_sensor_timestamp / 1000,
 						   "eff ctrl3     ",
-						   actuators.control_effective[3]);
+						   actuators_0.control_effective[3]);
 	}
 }
 
@@ -626,6 +624,22 @@ l_home(const struct listener *l)
 	mavlink_msg_gps_global_origin_send(MAVLINK_COMM_0, home.lat, home.lon, home.alt);
 }
 
+void
+l_airspeed(const struct listener *l)
+{
+	struct airspeed_s airspeed;
+
+	orb_copy(ORB_ID(airspeed), mavlink_subs.airspeed_sub, &airspeed);
+
+	float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
+	float throttle = actuators_0.control_effective[3] * (UINT16_MAX - 1);
+	float alt = global_pos.alt;
+	float climb = global_pos.vz;
+
+	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed,
+		((att.yaw + M_PI_F) / M_PI_F) * 180.0f, throttle, alt, climb);
+}
+
 static void *
 uorb_receive_thread(void *arg)
 {
@@ -765,6 +779,10 @@ uorb_receive_start(void)
 	mavlink_subs.optical_flow = orb_subscribe(ORB_ID(optical_flow));
 	orb_set_interval(mavlink_subs.optical_flow, 200); 	/* 5Hz updates */
 
+	/* --- AIRSPEED / VFR / HUD --- */
+	mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+	orb_set_interval(mavlink_subs.airspeed_sub, 200); 	/* 5Hz updates */
+
 	/* start the listener loop */
 	pthread_attr_t uorb_attr;
 	pthread_attr_init(&uorb_attr);
diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h
index d61cd43dc..73e278dc6 100644
--- a/src/modules/mavlink/orb_topics.h
+++ b/src/modules/mavlink/orb_topics.h
@@ -52,6 +52,7 @@
 #include <uORB/topics/vehicle_local_position_setpoint.h>
 #include <uORB/topics/vehicle_vicon_position.h>
 #include <uORB/topics/vehicle_global_position_setpoint.h>
+#include <uORB/topics/vehicle_global_position_set_triplet.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -59,7 +60,9 @@
 #include <uORB/topics/actuator_controls_effective.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/manual_control_setpoint.h>
+#include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/airspeed.h>
 #include <drivers/drv_rc_input.h>
 
 struct mavlink_subscriptions {
@@ -83,6 +86,7 @@ struct mavlink_subscriptions {
 	int optical_flow;
 	int rates_setpoint_sub;
 	int home_sub;
+	int airspeed_sub;
 };
 
 extern struct mavlink_subscriptions mavlink_subs;
diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c
index a131b143b..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"
@@ -347,15 +348,24 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 {
 	static uint16_t counter;
 
-	// Do not flood the precious wireless link with debug data
-	// if (wpm->size > 0 && counter % 10 == 0) {
-	// 	printf("Currect active waypoint id: %i\n", wpm->current_active_wp_id);
-	// }
+	if (wpm->current_active_wp_id < wpm->size) {
 
+		float orbit;
+		if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT) {
 
-	if (wpm->current_active_wp_id < wpm->size) {
+			orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
+
+		} else if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) {
+
+			orbit = wpm->waypoints[wpm->current_active_wp_id].param3;
+		} else {
+
+			// XXX set default orbit via param
+			orbit = 15.0f;
+		}
 
-		float orbit = wpm->waypoints[wpm->current_active_wp_id].param2;
 		int coordinate_frame = wpm->waypoints[wpm->current_active_wp_id].frame;
 		float dist = -1.0f;
 
@@ -374,10 +384,9 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 		}
 
 		if (dist >= 0.f && dist <= orbit /*&& wpm->yaw_reached*/) { //TODO implement yaw
+
 			wpm->pos_reached = true;
 
-			if (counter % 100 == 0)
-				printf("Setpoint reached: %0.4f, orbit: %.4f\n", dist, orbit);
 		}
 
 //		else
@@ -394,29 +403,47 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_
 
 			if (wpm->timestamp_firstinside_orbit == 0) {
 				// Announce that last waypoint was reached
-				printf("Reached waypoint %u for the first time \n", cur_wp->seq);
 				mavlink_wpm_send_waypoint_reached(cur_wp->seq);
 				wpm->timestamp_firstinside_orbit = now;
 			}
 
 			// check if the MAV was long enough inside the waypoint orbit
 			//if (now-timestamp_lastoutside_orbit > (cur_wp->hold_time*1000))
-			if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param2 * 1000) {
-				printf("Reached waypoint %u long enough \n", cur_wp->seq);
 
+			bool time_elapsed = false;
+
+			if (cur_wp->command == (int)MAV_CMD_NAV_LOITER_TIME) {
+				if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
+					time_elapsed = true;
+				}
+			} else if (now - wpm->timestamp_firstinside_orbit >= cur_wp->param1 * 1000 * 1000) {
+				time_elapsed = true;
+			} else if (cur_wp->command == (int)MAV_CMD_NAV_TAKEOFF) {
+				time_elapsed = true;
+			}
+
+			if (time_elapsed) {
 				if (cur_wp->autocontinue) {
 					cur_wp->current = 0;
 
-					if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
-						/* the last waypoint was reached, if auto continue is
-						 * activated restart the waypoint list from the beginning
-						 */
-						wpm->current_active_wp_id = 0;
+					/* only accept supported navigation waypoints, skip unknown ones */
+					do {
 
-					} else {
-						if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
-							wpm->current_active_wp_id++;
-					}
+						if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) {
+							/* the last waypoint was reached, if auto continue is
+							 * activated restart the waypoint list from the beginning
+							 */
+							wpm->current_active_wp_id = 0;
+
+						} else {
+							if ((uint16_t)(wpm->current_active_wp_id + 1) < wpm->size)
+								wpm->current_active_wp_id++;
+						}
+			
+					} while (!(wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT ||
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS ||
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME ||
+				wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM));
 
 					// Fly to next waypoint
 					wpm->timestamp_firstinside_orbit = 0;
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 5a2685560..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>
@@ -495,7 +494,7 @@ int mavlink_onboard_main(int argc, char *argv[])
 			errx(0, "mavlink already running\n");
 
 		thread_should_exit = false;
-		mavlink_task = task_spawn("mavlink_onboard",
+		mavlink_task = task_spawn_cmd("mavlink_onboard",
 					  SCHED_DEFAULT,
 					  SCHED_PRIORITY_DEFAULT,
 					  2048,
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/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c
index d94c0a69c..99f25cfe9 100644
--- a/src/modules/multirotor_att_control/multirotor_att_control_main.c
+++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c
@@ -466,7 +466,7 @@ int multirotor_att_control_main(int argc, char *argv[])
 	if (!strcmp(argv[1 + optioncount], "start")) {
 
 		thread_should_exit = false;
-		mc_task = task_spawn("multirotor_att_control",
+		mc_task = task_spawn_cmd("multirotor_att_control",
 				     SCHED_DEFAULT,
 				     SCHED_PRIORITY_MAX - 15,
 				     2048,
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c
index 76dbb36d3..8f19c6a4b 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.c
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c
@@ -1,12 +1,12 @@
 /****************************************************************************
  *
  *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *           Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,7 +39,15 @@
 
 /*
  * @file multirotor_attitude_control.c
- * Implementation of attitude controller
+ *
+ * Implementation of attitude controller for multirotors.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #include "multirotor_attitude_control.h"
@@ -163,16 +171,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 	static uint64_t last_run = 0;
 	static uint64_t last_input = 0;
 	float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-	float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
 	last_run = hrt_absolute_time();
 
 	if (last_input != att_sp->timestamp) {
 		last_input = att_sp->timestamp;
 	}
 
-	static int sensor_delay;
-	sensor_delay = hrt_absolute_time() - att->timestamp;
-
 	static int motor_skip_counter = 0;
 
 	static PID_t pitch_controller;
@@ -190,10 +194,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s
 		parameters_init(&h);
 		parameters_update(&h, &p);
 
-		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
-			 1000.0f, PID_MODE_DERIVATIV_SET);
-		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f,
-			 1000.0f, PID_MODE_DERIVATIV_SET);
+		pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
+		pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET, 0.0f);
 
 		initialized = true;
 	}
diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.h b/src/modules/multirotor_att_control/multirotor_attitude_control.h
index 2cf83e443..e78f45c47 100644
--- a/src/modules/multirotor_att_control/multirotor_attitude_control.h
+++ b/src/modules/multirotor_att_control/multirotor_attitude_control.h
@@ -1,12 +1,12 @@
 /****************************************************************************
  *
  *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *           Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,7 +39,15 @@
 
 /*
  * @file multirotor_attitude_control.h
- * Attitude control for multi rotors.
+ *
+ * Definition of attitude controller for multirotors.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef MULTIROTOR_ATTITUDE_CONTROL_H_
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index deba1ac03..e58d357d5 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -1,8 +1,10 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
  *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
  *           Lorenz Meier <lm@inf.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,10 +38,12 @@
 /**
  * @file multirotor_rate_control.c
  *
- * Implementation of rate controller
+ * Implementation of rate controller for multirotors.
  *
  * @author Tobias Naegeli <naegelit@student.ethz.ch>
  * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #include "multirotor_rate_control.h"
@@ -150,14 +154,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 			      const float rates[], struct actuator_controls_s *actuators)
 {
-	static float roll_control_last  = 0;
-	static float pitch_control_last = 0;
 	static uint64_t last_run = 0;
 	const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
 	static uint64_t last_input = 0;
 
-	float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f;
-
 	if (last_input != rate_sp->timestamp) {
 		last_input = rate_sp->timestamp;
 	}
@@ -166,6 +166,9 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	static int motor_skip_counter = 0;
 
+	static PID_t pitch_rate_controller;
+	static PID_t roll_rate_controller;
+
 	static struct mc_rate_control_params p;
 	static struct mc_rate_control_param_handles h;
 
@@ -176,43 +179,35 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		parameters_init(&h);
 		parameters_update(&h, &p);
 		initialized = true;
+
+		pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
+		pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
+
 	}
 
 	/* load new parameters with lower rate */
 	if (motor_skip_counter % 2500 == 0) {
 		/* update parameters from storage */
 		parameters_update(&h, &p);
-		// warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz",
-		// 	(double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input));
+		pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
+		pid_set_parameters(&roll_rate_controller,  p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f);
 	}
 
-	/* calculate current control outputs */
+	/* reset integral if on ground */
+	if (rate_sp->thrust < 0.01f) {
+		pid_reset_integral(&pitch_rate_controller);
+		pid_reset_integral(&roll_rate_controller);
+	}
 
 	/* control pitch (forward) output */
-	float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last);
-
-	/* increase resilience to faulty control inputs */
-	if (isfinite(pitch_control)) {
-		pitch_control_last = pitch_control;
-
-	} else {
-		pitch_control = 0.0f;
-		warnx("rej. NaN ctrl pitch");
-	}
+	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
+					    rates[1], 0.0f, deltaT);
 
 	/* control roll (left/right) output */
-	float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last);
-
-	/* increase resilience to faulty control inputs */
-	if (isfinite(roll_control)) {
-		roll_control_last = roll_control;
-
-	} else {
-		roll_control = 0.0f;
-		warnx("rej. NaN ctrl roll");
-	}
+	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
+					   rates[0], 0.0f, deltaT);
 
-	/* control yaw rate */
+	/* control yaw rate */ //XXX use library here
 	float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]);
 
 	/* increase resilience to faulty control inputs */
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h
index 03dec317a..362b5ed86 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.h
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.h
@@ -1,12 +1,12 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *   Author: Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           Julian Oes <joes@student.ethz.ch>
+ *           Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -39,7 +39,15 @@
 
 /*
  * @file multirotor_attitude_control.h
- * Attitude control for multi rotors.
+ *
+ * Definition of rate controller for multirotors.
+ *
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Julian Oes <joes@student.ethz.ch>
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #ifndef MULTIROTOR_RATE_CONTROL_H_
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 7b8d83aa8..f39d11438 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -94,7 +94,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 multirotor_pos_control_main(int argc, char *argv[])
 {
@@ -110,7 +110,7 @@ int multirotor_pos_control_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn("multirotor pos control",
+		deamon_task = task_spawn_cmd("multirotor pos control",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 60,
 					 4096,
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c
index 10dee3f22..984bd1329 100755
--- a/src/modules/position_estimator_mc/position_estimator_mc_main.c
+++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c
@@ -123,7 +123,7 @@ int position_estimator_mc_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		position_estimator_mc_task = task_spawn("position_estimator_mc",
+		position_estimator_mc_task = task_spawn_cmd("position_estimator_mc",
 					 SCHED_RR,
 					 SCHED_PRIORITY_MAX - 5,
 					 4096,
diff --git a/src/modules/px4iofirmware/adc.c b/src/modules/px4iofirmware/adc.c
index f744698be..81566eb2a 100644
--- a/src/modules/px4iofirmware/adc.c
+++ b/src/modules/px4iofirmware/adc.c
@@ -41,7 +41,7 @@
 
 #include <nuttx/arch.h>
 #include <arch/stm32/chip.h>
-#include <stm32_internal.h>
+#include <stm32.h>
 
 #include <drivers/drv_hrt.h>
 #include <systemlib/perf_counter.h>
diff --git a/src/modules/sdlog/sdlog.c b/src/modules/sdlog/sdlog.c
index 84a9eb6ac..c22523bf2 100644
--- a/src/modules/sdlog/sdlog.c
+++ b/src/modules/sdlog/sdlog.c
@@ -161,7 +161,7 @@ bool logging_enabled = true;
  * 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 sdlog_main(int argc, char *argv[])
 {
@@ -177,7 +177,7 @@ int sdlog_main(int argc, char *argv[])
 		}
 
 		thread_should_exit = false;
-		deamon_task = task_spawn("sdlog",
+		deamon_task = task_spawn_cmd("sdlog",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT - 30,
 					 4096,
diff --git a/src/modules/sdlog2/logbuffer.c b/src/modules/sdlog2/logbuffer.c
index 2e1e4fd4d..b3243f7b5 100644
--- a/src/modules/sdlog2/logbuffer.c
+++ b/src/modules/sdlog2/logbuffer.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <rk3dov@gmail.com>
+ *   Author: 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
@@ -37,7 +37,7 @@
  *
  * Ring FIFO buffer for binary log data.
  *
- * @author Anton Babushkin <rk3dov@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <string.h>
diff --git a/src/modules/sdlog2/logbuffer.h b/src/modules/sdlog2/logbuffer.h
index 31521f722..3a5e3a29f 100644
--- a/src/modules/sdlog2/logbuffer.h
+++ b/src/modules/sdlog2/logbuffer.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <rk3dov@gmail.com>
+ *   Author: 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
@@ -37,7 +37,7 @@
  *
  * Ring FIFO buffer for binary log data.
  *
- * @author Anton Babushkin <rk3dov@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef SDLOG2_RINGBUFFER_H_
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index c543fb1b4..3e6b20472 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -2,7 +2,7 @@
  *
  *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *           Anton Babushkin <rk3dov@gmail.com>
+ *           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
@@ -40,7 +40,7 @@
  * does the heavy SD I/O in a low-priority worker thread.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Anton Babushkin <rk3dov@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #include <nuttx/config.h>
@@ -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>
 
@@ -237,7 +238,7 @@ int sdlog2_main(int argc, char *argv[])
 		}
 
 		main_thread_should_exit = false;
-		deamon_task = task_spawn("sdlog2",
+		deamon_task = task_spawn_cmd("sdlog2",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_DEFAULT - 30,
 					 3000,
@@ -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 */
@@ -684,6 +687,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_OUT0_s log_OUT0;
 			struct log_AIRS_s log_AIRS;
 			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)
@@ -793,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.
@@ -1055,7 +1067,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* --- GLOBAL POSITION --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_global_position), subs.global_pos_sub, &buf.global_pos);
-				// TODO not implemented yet
+				log_msg.msg_type = LOG_GPOS_MSG;
+				log_msg.body.log_GPOS.lat = buf.global_pos.lat;
+				log_msg.body.log_GPOS.lon = buf.global_pos.lon;
+				log_msg.body.log_GPOS.alt = buf.global_pos.alt;
+				log_msg.body.log_GPOS.vel_n = buf.global_pos.vx;
+				log_msg.body.log_GPOS.vel_e = buf.global_pos.vy;
+				log_msg.body.log_GPOS.vel_d = buf.global_pos.vz;
+				LOGBUFFER_WRITE_AND_COUNT(GPOS);
 			}
 
 			/* --- VICON POSITION --- */
@@ -1067,7 +1086,15 @@ int sdlog2_thread_main(int argc, char *argv[])
 			/* --- FLOW --- */
 			if (fds[ifds++].revents & POLLIN) {
 				orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow);
-				// TODO not implemented yet
+				log_msg.msg_type = LOG_FLOW_MSG;
+				log_msg.body.log_FLOW.flow_raw_x = buf.flow.flow_raw_x;
+				log_msg.body.log_FLOW.flow_raw_y = buf.flow.flow_raw_y;
+				log_msg.body.log_FLOW.flow_comp_x = buf.flow.flow_comp_x_m;
+				log_msg.body.log_FLOW.flow_comp_y = buf.flow.flow_comp_y_m;
+				log_msg.body.log_FLOW.distance = buf.flow.ground_distance_m;
+				log_msg.body.log_FLOW.quality = buf.flow.quality;
+				log_msg.body.log_FLOW.sensor_id = buf.flow.sensor_id;
+				LOGBUFFER_WRITE_AND_COUNT(FLOW);
 			}
 
 			/* --- RC CHANNELS --- */
@@ -1088,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_format.h b/src/modules/sdlog2/sdlog2_format.h
index 59b91d90d..5c175ef7e 100644
--- a/src/modules/sdlog2/sdlog2_format.h
+++ b/src/modules/sdlog2/sdlog2_format.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <rk3dov@gmail.com>
+ *   Author: 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
@@ -37,7 +37,7 @@
  *
  * General log format structures and macro.
  *
- * @author Anton Babushkin <rk3dov@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 /*
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 7f7bf6053..abc882d23 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
  *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <rk3dov@gmail.com>
+ *   Author: 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
@@ -37,7 +37,7 @@
  *
  * Log messages and structures definition.
  *
- * @author Anton Babushkin <rk3dov@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
  */
 
 #ifndef SDLOG2_MESSAGES_H_
@@ -186,6 +186,47 @@ struct log_ARSP_s {
 	float pitch_rate_sp;
 	float yaw_rate_sp;
 };
+
+/* --- FLOW - OPTICAL FLOW --- */
+#define LOG_FLOW_MSG 15
+struct log_FLOW_s {
+	int16_t flow_raw_x;
+	int16_t flow_raw_y;
+	float flow_comp_x;
+	float flow_comp_y;
+	float distance;
+	uint8_t	quality;
+	uint8_t sensor_id;
+};
+
+/* --- GPOS - GLOBAL POSITION ESTIMATE --- */
+#define LOG_GPOS_MSG 16
+struct log_GPOS_s {
+	int32_t lat;
+	int32_t lon;
+	float alt;
+	float vel_n;
+	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 */
@@ -205,6 +246,9 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
 	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 230060148..f6f4d60c7 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -48,6 +48,10 @@ PARAM_DEFINE_FLOAT(SENS_GYRO_XOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_YOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_GYRO_ZOFF, 0.0f);
 
+PARAM_DEFINE_FLOAT(SENS_GYRO_XSCALE, 1.0f);
+PARAM_DEFINE_FLOAT(SENS_GYRO_YSCALE, 1.0f);
+PARAM_DEFINE_FLOAT(SENS_GYRO_ZSCALE, 1.0f);
+
 PARAM_DEFINE_FLOAT(SENS_MAG_XOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_MAG_YOFF, 0.0f);
 PARAM_DEFINE_FLOAT(SENS_MAG_ZOFF, 0.0f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 6b6aeedee..1ded14a91 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -194,6 +194,7 @@ private:
 		float scaling_factor[_rc_max_chan_count];
 
 		float gyro_offset[3];
+		float gyro_scale[3];
 		float mag_offset[3];
 		float mag_scale[3];
 		float accel_offset[3];
@@ -243,6 +244,7 @@ private:
 		param_t rc_demix;
 
 		param_t gyro_offset[3];
+		param_t gyro_scale[3];
 		param_t accel_offset[3];
 		param_t accel_scale[3];
 		param_t mag_offset[3];
@@ -486,6 +488,9 @@ Sensors::Sensors() :
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
 	_parameter_handles.gyro_offset[1] = param_find("SENS_GYRO_YOFF");
 	_parameter_handles.gyro_offset[2] = param_find("SENS_GYRO_ZOFF");
+	_parameter_handles.gyro_scale[0] = param_find("SENS_GYRO_XSCALE");
+	_parameter_handles.gyro_scale[1] = param_find("SENS_GYRO_YSCALE");
+	_parameter_handles.gyro_scale[2] = param_find("SENS_GYRO_ZSCALE");
 
 	/* accel offsets */
 	_parameter_handles.accel_offset[0] = param_find("SENS_ACC_XOFF");
@@ -696,6 +701,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0]));
 	param_get(_parameter_handles.gyro_offset[1], &(_parameters.gyro_offset[1]));
 	param_get(_parameter_handles.gyro_offset[2], &(_parameters.gyro_offset[2]));
+	param_get(_parameter_handles.gyro_scale[0], &(_parameters.gyro_scale[0]));
+	param_get(_parameter_handles.gyro_scale[1], &(_parameters.gyro_scale[1]));
+	param_get(_parameter_handles.gyro_scale[2], &(_parameters.gyro_scale[2]));
 
 	/* accel offsets */
 	param_get(_parameter_handles.accel_offset[0], &(_parameters.accel_offset[0]));
@@ -983,11 +991,11 @@ Sensors::parameter_update_poll(bool forced)
 		int fd = open(GYRO_DEVICE_PATH, 0);
 		struct gyro_scale gscale = {
 			_parameters.gyro_offset[0],
-			1.0f,
+			_parameters.gyro_scale[0],
 			_parameters.gyro_offset[1],
-			1.0f,
+			_parameters.gyro_scale[1],
 			_parameters.gyro_offset[2],
-			1.0f,
+			_parameters.gyro_scale[2],
 		};
 
 		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
@@ -1445,7 +1453,7 @@ Sensors::start()
 	ASSERT(_sensors_task == -1);
 
 	/* start the task */
-	_sensors_task = task_spawn("sensors_task",
+	_sensors_task = task_spawn_cmd("sensors_task",
 				   SCHED_DEFAULT,
 				   SCHED_PRIORITY_MAX - 5,
 				   2048,
@@ -1502,6 +1510,7 @@ int sensors_main(int argc, char *argv[])
 		}
 	}
 
-	errx(1, "unrecognized command");
+	warnx("unrecognized command");
+	return 1;
 }
 
diff --git a/src/modules/systemlib/conversions.h b/src/modules/systemlib/conversions.h
index 5d485b01f..064426f21 100644
--- a/src/modules/systemlib/conversions.h
+++ b/src/modules/systemlib/conversions.h
@@ -43,11 +43,7 @@
 #define CONVERSIONS_H_
 #include <float.h>
 #include <stdint.h>
-
-#define CONSTANTS_ONE_G						9.80665f		// m/s^2
-#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C	1.225f			// kg/m^3
-#define CONSTANTS_AIR_GAS_CONST				287.1f 			// J/(kg * K)
-#define CONSTANTS_ABSOLUTE_NULL_CELSIUS		-273.15f		// °C
+#include <systemlib/geo/geo.h>
 
 __BEGIN_DECLS
 
diff --git a/src/modules/systemlib/cpuload.c b/src/modules/systemlib/cpuload.c
index 20b711fa6..8fdff8ac0 100644
--- a/src/modules/systemlib/cpuload.c
+++ b/src/modules/systemlib/cpuload.c
@@ -1,9 +1,8 @@
 /****************************************************************************
- * configs/px4fmu/src/up_leds.c
- * arch/arm/src/board/up_leds.c
  *
- *   Copyright (C) 2011 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gnutt@nuttx.org>
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
+ * 	     Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -15,7 +14,7 @@
  *    notice, this list of conditions and the following disclaimer in
  *    the documentation and/or other materials provided with the
  *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
+ * 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.
  *
@@ -34,18 +33,26 @@
  *
  ****************************************************************************/
 
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
+/**
+ * @file cpuload.c
+ *
+ * Measurement of CPU load of each individual task.
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
+ */
 #include <nuttx/config.h>
+#include <nuttx/sched.h>
 
+#include <sys/types.h>
 #include <stdint.h>
 #include <stdbool.h>
+
+#include <arch/arch.h>
+
 #include <debug.h>
 
 #include <sys/time.h>
-#include <sched.h>
 
 #include <arch/board/board.h>
 #include <drivers/drv_hrt.h>
@@ -54,26 +61,13 @@
 
 #ifdef CONFIG_SCHED_INSTRUMENTATION
 
-/****************************************************************************
- * Definitions
- ****************************************************************************/
-
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-__EXPORT void sched_note_start(FAR _TCB *tcb);
-__EXPORT void sched_note_stop(FAR _TCB *tcb);
-__EXPORT void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb);
-
-/****************************************************************************
- * Name:
- ****************************************************************************/
+__EXPORT void sched_note_start(FAR struct tcb_s *tcb);
+__EXPORT void sched_note_stop(FAR struct tcb_s *tcb);
+__EXPORT void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb);
 
 __EXPORT struct system_load_s system_load;
 
-extern FAR _TCB *sched_gettcb(pid_t pid);
+extern FAR struct _TCB *sched_gettcb(pid_t pid);
 
 void cpuload_initialize_once()
 {
@@ -109,7 +103,7 @@ void cpuload_initialize_once()
 	//	}
 }
 
-void sched_note_start(FAR _TCB *tcb)
+void sched_note_start(FAR struct tcb_s *tcb)
 {
 	/* search first free slot */
 	int i;
@@ -128,7 +122,7 @@ void sched_note_start(FAR _TCB *tcb)
 	}
 }
 
-void sched_note_stop(FAR _TCB *tcb)
+void sched_note_stop(FAR struct tcb_s *tcb)
 {
 	int i;
 
@@ -145,7 +139,7 @@ void sched_note_stop(FAR _TCB *tcb)
 	}
 }
 
-void sched_note_switch(FAR _TCB *pFromTcb, FAR _TCB *pToTcb)
+void sched_note_switch(FAR struct tcb_s *pFromTcb, FAR struct tcb_s *pToTcb)
 {
 	uint64_t new_time = hrt_absolute_time();
 
diff --git a/src/modules/systemlib/cpuload.h b/src/modules/systemlib/cpuload.h
index a97047ea8..c7aa18d3c 100644
--- a/src/modules/systemlib/cpuload.h
+++ b/src/modules/systemlib/cpuload.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -43,7 +43,7 @@ struct system_load_taskinfo_s {
 	uint64_t total_runtime;      ///< Runtime since start (start_time - total_runtime)/(start_time - current_time) = load
 	uint64_t curr_start_time;     ///< Start time of the current scheduling slot
 	uint64_t start_time;         ///< FIRST start time of task
-	FAR struct _TCB *tcb;               ///<
+	FAR struct tcb_s *tcb;               ///<
 	bool valid;                  ///< Task is currently active / valid
 };
 
@@ -60,4 +60,6 @@ __EXPORT extern struct system_load_s system_load;
 
 __EXPORT void cpuload_initialize_once(void);
 
+__END_DECLS
+
 #endif
diff --git a/src/modules/systemlib/err.c b/src/modules/systemlib/err.c
index daf17ef8b..6c0e876d1 100644
--- a/src/modules/systemlib/err.c
+++ b/src/modules/systemlib/err.c
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -61,7 +61,7 @@ const char *
 getprogname(void)
 {
 #if CONFIG_TASK_NAME_SIZE > 0
-	_TCB	*thisproc = sched_self();
+	FAR struct tcb_s	*thisproc = sched_self();
 
 	return thisproc->name;
 #else
diff --git a/src/modules/systemlib/geo/geo.h b/src/modules/systemlib/geo/geo.h
index 84097b49f..dadec51ec 100644
--- a/src/modules/systemlib/geo/geo.h
+++ b/src/modules/systemlib/geo/geo.h
@@ -45,9 +45,23 @@
  * Additional functions - @author Doug Weibel <douglas.weibel@colorado.edu>
  */
 
+#pragma once
+
+__BEGIN_DECLS
 
 #include <stdbool.h>
 
+#define CONSTANTS_ONE_G					9.80665f		/* m/s^2		*/
+#define CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C		1.225f			/* kg/m^3		*/
+#define CONSTANTS_AIR_GAS_CONST				287.1f 			/* J/(kg * K)		*/
+#define CONSTANTS_ABSOLUTE_NULL_CELSIUS			-273.15f		/* °C			*/
+#define CONSTANTS_RADIUS_OF_EARTH			6371000			/* meters (m)		*/
+
+/* compatibility aliases */
+#define RADIUS_OF_EARTH		CONSTANTS_RADIUS_OF_EARTH
+#define GRAVITY_MSS		CONSTANTS_ONE_G
+
+// XXX remove
 struct crosstrack_error_s {
 	bool past_end;		// Flag indicating we are past the end of the line/arc segment
 	float distance;		// Distance in meters to closest point on line/arc
@@ -111,3 +125,5 @@ __EXPORT float _wrap_180(float bearing);
 __EXPORT float _wrap_360(float bearing);
 __EXPORT float _wrap_pi(float bearing);
 __EXPORT float _wrap_2pi(float bearing);
+
+__END_DECLS
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 49315cdc9..4996a8f66 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -1,9 +1,11 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,14 +38,21 @@
 
 /**
  * @file pid.c
- * Implementation of generic PID control interface
+ *
+ * Implementation of generic PID control interface.
+ *
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #include "pid.h"
 #include <math.h>
 
 __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
-		       float limit, uint8_t mode)
+		       float limit, uint8_t mode, float dt_min)
 {
 	pid->kp = kp;
 	pid->ki = ki;
@@ -51,13 +60,13 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
 	pid->intmax = intmax;
 	pid->limit = limit;
 	pid->mode = mode;
-	pid->count = 0;
-	pid->saturated = 0;
-	pid->last_output = 0;
-
-	pid->sp = 0;
-	pid->error_previous = 0;
-	pid->integral = 0;
+	pid->dt_min = dt_min;
+	pid->count = 0.0f;
+	pid->saturated = 0.0f;
+	pid->last_output = 0.0f;
+	pid->sp = 0.0f;
+	pid->error_previous = 0.0f;
+	pid->integral = 0.0f;
 }
 __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit)
 {
@@ -136,14 +145,14 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	// Calculated current error value
 	float error = pid->sp - val;
 
-	if (isfinite(error)) {				// Why is this necessary?  DEW
-		pid->error_previous = error;
-	}
-
 	// Calculate or measured current error derivative
-
 	if (pid->mode == PID_MODE_DERIVATIV_CALC) {
-		d = (error - pid->error_previous) / dt;
+		d = (error - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = error;
+
+	} else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) {
+		d = (-val - pid->error_previous) / fmaxf(dt, pid->dt_min);
+		pid->error_previous = -val;
 
 	} else if (pid->mode == PID_MODE_DERIVATIV_SET) {
 		d = -val_dot;
@@ -152,6 +161,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 		d = 0.0f;
 	}
 
+	if (!isfinite(d)) {
+		d = 0.0f;
+	}
+
 	// Calculate the error integral and check for saturation
 	i = pid->integral + (error * dt);
 
@@ -162,7 +175,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 
 	} else {
 		if (!isfinite(i)) {
-			i = 0;
+			i = 0.0f;
 		}
 
 		pid->integral = i;
@@ -170,17 +183,19 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	}
 
 	// Calculate the output.  Limit output magnitude to pid->limit
-	float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd);
+	float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
 
-	if (output > pid->limit) output = pid->limit;
+	if (isfinite(output)) {
+		if (output > pid->limit) {
+			output = pid->limit;
 
-	if (output < -pid->limit) output = -pid->limit;
+		} else if (output < -pid->limit) {
+			output = -pid->limit;
+		}
 
-	if (isfinite(output)) {
 		pid->last_output = output;
 	}
 
-
 	return pid->last_output;
 }
 
diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h
index 64d668867..eca228464 100644
--- a/src/modules/systemlib/pid/pid.h
+++ b/src/modules/systemlib/pid/pid.h
@@ -1,9 +1,11 @@
 /****************************************************************************
  *
- *   Copyright (C) 2008-2012 PX4 Development Team. All rights reserved.
- *   Author: @author Laurens Mackay <mackayl@student.ethz.ch>
- *           @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ *   Copyright (C) 2008-2013 PX4 Development Team. All rights reserved.
+ *   Author: Laurens Mackay <mackayl@student.ethz.ch>
+ *           Tobias Naegeli <naegelit@student.ethz.ch>
+ *           Martin Rutschmann <rutmarti@student.ethz.ch>
+ *           Anton Babushkin <anton.babushkin@me.com>
+ *           Julian Oes <joes@student.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,7 +38,14 @@
 
 /**
  * @file pid.h
- * Definition of generic PID control interface
+ *
+ * Definition of generic PID control interface.
+ *
+ * @author Laurens Mackay <mackayl@student.ethz.ch>
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Martin Rutschmann <rutmarti@student.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Julian Oes <joes@student.ethz.ch>
  */
 
 #ifndef PID_H_
@@ -44,11 +53,16 @@
 
 #include <stdint.h>
 
+__BEGIN_DECLS
+
 /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error
  * val_dot in pid_calculate() will be ignored */
 #define PID_MODE_DERIVATIV_CALC	0
+/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored
+ * val_dot in pid_calculate() will be ignored */
+#define PID_MODE_DERIVATIV_CALC_NO_SP	1
 /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */
-#define PID_MODE_DERIVATIV_SET	1
+#define PID_MODE_DERIVATIV_SET	2
 // Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID)
 #define PID_MODE_DERIVATIV_NONE 9
 
@@ -62,17 +76,19 @@ typedef struct {
 	float error_previous;
 	float last_output;
 	float limit;
+	float dt_min;
 	uint8_t mode;
 	uint8_t count;
 	uint8_t saturated;
 } PID_t;
 
-__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode);
+__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode, float dt_min);
 __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit);
 //void pid_set(PID_t *pid, float sp);
 __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt);
 
 __EXPORT void pid_reset_integral(PID_t *pid);
 
+__END_DECLS
 
 #endif /* PID_H_ */
diff --git a/src/modules/systemlib/systemlib.c b/src/modules/systemlib/systemlib.c
index afb7eca29..3283aad8a 100644
--- a/src/modules/systemlib/systemlib.c
+++ b/src/modules/systemlib/systemlib.c
@@ -50,7 +50,7 @@
 
 #include "systemlib.h"
 
-static void kill_task(FAR _TCB *tcb, FAR void *arg);
+static void kill_task(FAR struct tcb_s *tcb, FAR void *arg);
 
 void killall()
 {
@@ -60,12 +60,12 @@ void killall()
 	sched_foreach(kill_task, NULL);
 }
 
-static void kill_task(FAR _TCB *tcb, FAR void *arg)
+static void kill_task(FAR struct tcb_s *tcb, FAR void *arg)
 {
 	kill(tcb->pid, SIGUSR1);
 }
 
-int task_spawn(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
+int task_spawn_cmd(const char *name, int scheduler, int priority, int stack_size, main_t entry, const char *argv[])
 {
 	int pid;
 
diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h
index 2c53c648b..0194b5e52 100644
--- a/src/modules/systemlib/systemlib.h
+++ b/src/modules/systemlib/systemlib.h
@@ -42,11 +42,11 @@
 #include <float.h>
 #include <stdint.h>
 
-__BEGIN_DECLS
-
 /** Reboots the board */
 extern void up_systemreset(void) noreturn_function;
 
+__BEGIN_DECLS
+
 /** Sends SIGUSR1 to all processes */
 __EXPORT void killall(void);
 
@@ -58,7 +58,7 @@ __EXPORT void killall(void);
 #endif
 
 /** Starts a task and performs any specific accounting, scheduler setup, etc. */
-__EXPORT int task_spawn(const char *name,
+__EXPORT int task_spawn_cmd(const char *name,
 			int priority,
 			int scheduler,
 			int stack_size,
diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp
index e22b58cad..ae5fc6c61 100644
--- a/src/modules/uORB/objects_common.cpp
+++ b/src/modules/uORB/objects_common.cpp
@@ -110,6 +110,9 @@ ORB_DEFINE(vehicle_global_position_setpoint, struct vehicle_global_position_setp
 #include "topics/vehicle_global_position_set_triplet.h"
 ORB_DEFINE(vehicle_global_position_set_triplet, struct vehicle_global_position_set_triplet_s);
 
+#include "topics/mission.h"
+ORB_DEFINE(mission, struct mission_s);
+
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
 
@@ -158,5 +161,14 @@ ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s);
 ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s);
 
+#include "topics/telemetry_status.h"
+ORB_DEFINE(telemetry_status, struct telemetry_status_s);
+
 #include "topics/debug_key_value.h"
 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/differential_pressure.h b/src/modules/uORB/topics/differential_pressure.h
index 8ce85213b..1ffeda764 100644
--- a/src/modules/uORB/topics/differential_pressure.h
+++ b/src/modules/uORB/topics/differential_pressure.h
@@ -52,10 +52,11 @@
  * Differential pressure.
  */
 struct differential_pressure_s {
-	uint64_t	timestamp;						/**< microseconds since system boot, needed to integrate */
-	uint16_t	differential_pressure_pa;		/**< Differential pressure reading */
+	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
+	uint16_t	differential_pressure_pa;	/**< Differential pressure reading */
 	uint16_t	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
-	float		voltage;						/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
+	float		voltage;			/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
+	float		temperature;			/**< Temperature provided by sensor */
 
 };
 
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/modules/uORB/topics/mission.h b/src/modules/uORB/topics/mission.h
new file mode 100644
index 000000000..253f444b3
--- /dev/null
+++ b/src/modules/uORB/topics/mission.h
@@ -0,0 +1,99 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Author: @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ *           @author Julian Oes <joes@student.ethz.ch>
+ *           @author Lorenz Meier <lm@inf.ethz.ch>
+ *
+ * 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 mission_item.h
+ * Definition of one mission item.
+ */
+
+#ifndef TOPIC_MISSION_H_
+#define TOPIC_MISSION_H_
+
+#include <stdint.h>
+#include <stdbool.h>
+#include "../uORB.h"
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+enum NAV_CMD {
+	NAV_CMD_WAYPOINT = 0,
+	NAV_CMD_LOITER_TURN_COUNT,
+	NAV_CMD_LOITER_TIME_LIMIT,
+	NAV_CMD_LOITER_UNLIMITED,
+	NAV_CMD_RETURN_TO_LAUNCH,
+	NAV_CMD_LAND,
+	NAV_CMD_TAKEOFF
+};
+
+/**
+ * Global position setpoint in WGS84 coordinates.
+ *
+ * This is the position the MAV is heading towards. If it of type loiter,
+ * the MAV is circling around it with the given loiter radius in meters.
+ */
+struct mission_item_s
+{
+	bool altitude_is_relative;	/**< true if altitude is relative from start point	*/
+	double lat;			/**< latitude in degrees * 1E7				*/
+	double lon;			/**< longitude in degrees * 1E7				*/
+	float altitude;			/**< altitude in meters					*/
+	float yaw;			/**< in radians NED -PI..+PI 				*/
+	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
+	uint8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
+	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
+	float param1;
+	float param2;
+	float param3;
+	float param4;
+};
+
+struct mission_s
+{
+	struct mission_item_s *items;
+	unsigned count;
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(mission);
+
+#endif
diff --git a/src/modules/uORB/topics/navigation_capabilities.h b/src/modules/uORB/topics/navigation_capabilities.h
new file mode 100644
index 000000000..6a3e811e3
--- /dev/null
+++ b/src/modules/uORB/topics/navigation_capabilities.h
@@ -0,0 +1,65 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012-2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file navigation_capabilities.h
+ *
+ * Definition of navigation capabilities uORB topic.
+ */
+
+#ifndef TOPIC_NAVIGATION_CAPABILITIES_H_
+#define TOPIC_NAVIGATION_CAPABILITIES_H_
+
+#include "../uORB.h"
+#include <stdint.h>
+
+/**
+ * @addtogroup topics
+ * @{
+ */
+
+/**
+ * Airspeed
+ */
+struct navigation_capabilities_s {
+    float turn_distance;    /**< the optimal distance to a waypoint to switch to the next */
+};
+
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
+ORB_DECLARE(navigation_capabilities);
+
+#endif
diff --git a/src/modules/uORB/topics/optical_flow.h b/src/modules/uORB/topics/optical_flow.h
index c854f0079..98f0e3fa2 100644
--- a/src/modules/uORB/topics/optical_flow.h
+++ b/src/modules/uORB/topics/optical_flow.h
@@ -57,8 +57,8 @@ struct optical_flow_s {
 
 	uint64_t timestamp;		/**< in microseconds since system start          */
 
-	uint16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
-	uint16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
+	int16_t flow_raw_x;		/**< flow in pixels in X direction, not rotation-compensated */
+	int16_t flow_raw_y;		/**< flow in pixels in Y direction, not rotation-compensated */
 	float flow_comp_x_m;		/**< speed over ground in meters, rotation-compensated */
 	float flow_comp_y_m;		/**< speed over ground in meters, rotation-compensated */
 	float ground_distance_m;	/**< Altitude / distance to ground in meters */
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
new file mode 100644
index 000000000..f30852de5
--- /dev/null
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -0,0 +1,67 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file telemetry_status.h
+ *
+ * Telemetry status topics - radio status outputs
+ */
+
+#ifndef TOPIC_TELEMETRY_STATUS_H
+#define TOPIC_TELEMETRY_STATUS_H
+
+#include <stdint.h>
+#include "../uORB.h"
+
+enum TELEMETRY_STATUS_RADIO_TYPE {
+    TELEMETRY_STATUS_RADIO_TYPE_GENERIC = 0,
+    TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO,
+    TELEMETRY_STATUS_RADIO_TYPE_UBIQUITY_BULLET,
+    TELEMETRY_STATUS_RADIO_TYPE_WIRE
+};
+
+struct telemetry_status_s {
+	uint64_t timestamp;
+    enum TELEMETRY_STATUS_RADIO_TYPE type;  /**< type of the radio hardware     */
+	unsigned rssi;              /**< local signal strength                      */
+    unsigned remote_rssi;       /**< remote signal strength                     */
+    unsigned rxerrors;          /**< receive errors                             */
+    unsigned fixed;             /**< count of error corrected packets           */
+    uint8_t noise;              /**< background noise level                     */
+    uint8_t remote_noise;       /**< remote background noise level              */
+    uint8_t txbuf;              /**< how full the tx buffer is as a percentage  */
+};
+
+ORB_DECLARE(telemetry_status);
+
+#endif /* TOPIC_TELEMETRY_STATUS_H */
\ No newline at end of file
diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
index eec6a8229..3ae3ff28c 100644
--- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h
@@ -45,6 +45,7 @@
 #include <stdint.h>
 #include <stdbool.h>
 #include "../uORB.h"
+#include "mission.h"
 
 /**
  * @addtogroup topics
@@ -65,7 +66,12 @@ struct vehicle_global_position_setpoint_s
 	float altitude;			/**< altitude in meters					*/
 	float yaw;			/**< in radians NED -PI..+PI 				*/
 	float loiter_radius;		/**< loiter radius in meters, 0 for a VTOL to hover     */
-	bool is_loiter;			/**< true if loitering is enabled			*/
+	uint8_t loiter_direction;	/**< 1: positive / clockwise, -1, negative.		*/
+	enum NAV_CMD nav_cmd;		/**< true if loitering is enabled			*/
+	float param1;
+	float param2;
+	float param3;
+	float param4;
 };
 
 /**
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index 4bcce18fb..7752ffe67 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/src/systemcmds/preflight_check/preflight_check.c
@@ -146,6 +146,15 @@ int preflight_check_main(int argc, char *argv[])
 	bool rc_ok = true;
 	char nbuf[20];
 
+	/* first check channel mappings */
+			/* check which map param applies */
+		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
+		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
+		// 	/* give system time to flush error message in case there are more */
+		// 	usleep(100000);
+		// 	count++;
+		// }
+
 	for (int i = 0; i < 12; i++) {
 		/* should the channel be enabled? */
 		uint8_t count = 0;
@@ -209,8 +218,8 @@ int preflight_check_main(int argc, char *argv[])
 			count++;
 		}
 
-		/* XXX needs inspection of all the _MAP params */
-		// if (conf[PX4IO_P_RC_CONFIG_ASSIGNMENT] >= MAX_CONTROL_CHANNELS) {
+		/* check which map param applies */
+		// if (map_by_channel[i] >= MAX_CONTROL_CHANNELS) {
 		// 	mavlink_log_critical(mavlink_fd, "ERR: RC_%d_MAP >= # CHANS", i+1);
 		// 	/* give system time to flush error message in case there are more */
 		// 	usleep(100000);
diff --git a/src/systemcmds/top/top.c b/src/systemcmds/top/top.c
index 59d2bc8f1..efe62685c 100644
--- a/src/systemcmds/top/top.c
+++ b/src/systemcmds/top/top.c
@@ -1,7 +1,7 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved.
+ *   Author: Lorenz Meier <lm@inf.ethz.ch>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -36,6 +36,8 @@
  * @file top.c
  * Tool similar to UNIX top command
  * @see http://en.wikipedia.org/wiki/Top_unix
+ * 
+ * @author Lorenz Meier <lm@inf.ethz.ch>
  */
 
 #include <nuttx/config.h>