Merge remote-tracking branch 'upstream/master' into fw_autoland_att_tecs

10 files changed, 322 insertions, 108 deletions

diff --git a/ROMFS/px4fmu_common/init.d/16_3dr_iris b/ROMFS/px4fmu_common/init.d/16_3dr_iris
index 6be917878..f6b071cf1 100644
--- a/ROMFS/px4fmu_common/init.d/16_3dr_iris
+++ b/ROMFS/px4fmu_common/init.d/16_3dr_iris
@@ -67,8 +67,7 @@ pwm rate -c 1234 -r 400
 # Set disarmed, min and max PWM signals
 #
 pwm disarmed -c 1234 -p 900
-pwm min -c 1234 -p 1200
-pwm max -c 1234 -p 1800
+pwm min -c 1234 -p 1050
 
 #
 # Start common for all multirotors apps
diff --git a/makefiles/config_px4fmu-v1_backside.mk b/makefiles/config_px4fmu-v1_backside.mk
new file mode 100644
index 000000000..e1a42530b
--- /dev/null
+++ b/makefiles/config_px4fmu-v1_backside.mk
@@ -0,0 +1,160 @@
+#
+# Makefile for the px4fmu_default configuration
+#
+
+#
+# Use the configuration's ROMFS.
+#
+ROMFS_ROOT	 = $(PX4_BASE)/ROMFS/px4fmu_common
+ROMFS_OPTIONAL_FILES = $(PX4_BASE)/Images/px4io-v1_default.bin
+
+#
+# Board support modules
+#
+MODULES		+= drivers/device
+MODULES		+= drivers/stm32
+MODULES		+= drivers/stm32/adc
+MODULES		+= drivers/stm32/tone_alarm
+MODULES		+= drivers/led
+MODULES		+= drivers/px4io
+MODULES		+= drivers/px4fmu
+MODULES		+= drivers/boards/px4fmu-v1
+MODULES		+= drivers/ardrone_interface
+MODULES		+= drivers/l3gd20
+MODULES		+= drivers/bma180
+MODULES		+= drivers/mpu6000
+MODULES		+= drivers/hmc5883
+MODULES		+= drivers/ms5611
+MODULES		+= drivers/mb12xx
+MODULES		+= drivers/gps
+MODULES		+= drivers/hil
+MODULES		+= drivers/hott/hott_telemetry
+MODULES		+= drivers/hott/hott_sensors
+MODULES		+= drivers/blinkm
+MODULES		+= drivers/rgbled
+MODULES		+= drivers/mkblctrl
+MODULES		+= drivers/roboclaw
+MODULES		+= drivers/airspeed
+MODULES		+= drivers/ets_airspeed
+MODULES		+= drivers/meas_airspeed
+MODULES		+= modules/sensors
+
+#
+# System commands
+#
+MODULES		+= systemcmds/eeprom
+MODULES		+= systemcmds/ramtron
+MODULES		+= systemcmds/bl_update
+MODULES		+= systemcmds/boardinfo
+MODULES		+= systemcmds/i2c
+MODULES		+= systemcmds/mixer
+MODULES		+= systemcmds/param
+MODULES		+= systemcmds/perf
+MODULES		+= systemcmds/preflight_check
+MODULES		+= systemcmds/pwm
+MODULES		+= systemcmds/esc_calib
+MODULES		+= systemcmds/reboot
+MODULES		+= systemcmds/top
+MODULES		+= systemcmds/tests
+MODULES		+= systemcmds/config
+MODULES		+= systemcmds/nshterm
+
+#
+# General system control
+#
+MODULES		+= modules/commander
+MODULES		+= modules/navigator
+MODULES		+= modules/mavlink
+MODULES		+= modules/mavlink_onboard
+MODULES		+= modules/gpio_led
+
+#
+# Estimation modules (EKF / other filters)
+#
+#MODULES		+= modules/attitude_estimator_ekf
+MODULES		+= modules/att_pos_estimator_ekf
+#MODULES		+= modules/position_estimator_inav
+MODULES		+= examples/flow_position_estimator
+
+#
+# Vehicle Control
+#
+#MODULES		+= modules/segway # XXX Needs GCC 4.7 fix
+#MODULES		+= modules/fw_pos_control_l1
+#MODULES		+= modules/fw_att_control
+#MODULES		+= modules/multirotor_att_control
+#MODULES		+= modules/multirotor_pos_control
+#MODULES		+= examples/flow_position_control
+#MODULES		+= examples/flow_speed_control
+MODULES 		+= modules/fixedwing_backside
+
+#
+# Logging
+#
+MODULES		+= modules/sdlog2
+
+#
+# Unit tests
+#
+#MODULES 	+= modules/unit_test
+#MODULES 	+= modules/commander/commander_tests
+
+#
+# Library modules
+#
+MODULES		+= modules/systemlib
+MODULES		+= modules/systemlib/mixer
+MODULES		+= modules/controllib
+MODULES		+= modules/uORB
+
+#
+# Libraries
+#
+LIBRARIES	+= lib/mathlib/CMSIS
+MODULES		+= lib/mathlib
+MODULES		+= lib/mathlib/math/filter
+MODULES		+= lib/ecl
+MODULES		+= lib/external_lgpl
+MODULES		+= lib/geo
+MODULES		+= lib/conversion
+
+#
+# Demo apps
+#
+#MODULES		+= examples/math_demo
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/hello_sky
+#MODULES		+= examples/px4_simple_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/daemon
+#MODULES		+= examples/px4_daemon_app
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/debug_values
+#MODULES		+= examples/px4_mavlink_debug
+
+# Tutorial code from
+# https://pixhawk.ethz.ch/px4/dev/example_fixedwing_control
+#MODULES			+= examples/fixedwing_control
+
+# Hardware test
+#MODULES			+= examples/hwtest
+
+#
+# Transitional support - add commands from the NuttX export archive.
+#
+# In general, these should move to modules over time.
+#
+# Each entry here is <command>.<priority>.<stacksize>.<entrypoint> but we use a helper macro
+# to make the table a bit more readable.
+#
+define _B
+	$(strip $1).$(or $(strip $2),SCHED_PRIORITY_DEFAULT).$(or $(strip $3),CONFIG_PTHREAD_STACK_DEFAULT).$(strip $4)
+endef
+
+#                  command                 priority                   stack  entrypoint
+BUILTIN_COMMANDS := \
+	$(call _B, sercon,                 ,                          2048,  sercon_main                ) \
+	$(call _B, serdis,                 ,                          2048,  serdis_main                ) \
+	$(call _B, sysinfo,                ,                          2048,  sysinfo_main               )
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 18fb6dcbc..ecca04dd7 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
@@ -41,6 +41,7 @@
 
 #include "KalmanNav.hpp"
 #include <systemlib/err.h>
+#include <geo/geo.h>
 
 // constants
 // Titterton pg. 52
@@ -67,15 +68,13 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	// 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
 	_param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz
 	// publications
 	_pos(&getPublications(), ORB_ID(vehicle_global_position)),
+	_localPos(&getPublications(), ORB_ID(vehicle_local_position)),
 	_att(&getPublications(), ORB_ID(vehicle_attitude)),
 	// timestamps
 	_pubTimeStamp(hrt_absolute_time()),
@@ -92,6 +91,7 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	phi(0), theta(0), psi(0),
 	vN(0), vE(0), vD(0),
 	lat(0), lon(0), alt(0),
+	lat0(0), lon0(0), alt0(0),
 	// parameters for ground station
 	_vGyro(this, "V_GYRO"),
 	_vAccel(this, "V_ACCEL"),
@@ -127,19 +127,15 @@ KalmanNav::KalmanNav(SuperBlock *parent, const char *name) :
 	lon = 0.0f;
 	alt = 0.0f;
 
-	// 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);
+	_sensors.update();
+	q = init(
+		_sensors.accelerometer_m_s2[0],
+		_sensors.accelerometer_m_s2[1],
+		_sensors.accelerometer_m_s2[2],
+		_sensors.magnetometer_ga[0],
+		_sensors.magnetometer_ga[1],
+		_sensors.magnetometer_ga[2]);
 
 	// initialize dcm
 	C_nb = Dcm(q);
@@ -252,11 +248,21 @@ void KalmanNav::update()
 		setLatDegE7(_gps.lat);
 		setLonDegE7(_gps.lon);
 		setAltE3(_gps.alt);
+		// set reference position for
+		// local position
+		lat0 = lat;
+		lon0 = lon;
+		alt0 = alt;
+		// XXX map_projection has internal global
+		// states that multiple things could change, 
+		// should make map_projection take reference
+		// lat/lon and not have init
+		map_projection_init(lat0, lon0);
 		_positionInitialized = true;
 		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);
+		       lat, lon, double(alt));
 	}
 
 	// prediction step
@@ -311,7 +317,7 @@ void KalmanNav::updatePublications()
 {
 	using namespace math;
 
-	// position publication
+	// global position publication
 	_pos.timestamp = _pubTimeStamp;
 	_pos.time_gps_usec = _gps.timestamp_position;
 	_pos.valid = true;
@@ -324,6 +330,31 @@ void KalmanNav::updatePublications()
 	_pos.vz = vD;
 	_pos.yaw = psi;
 
+	// local position publication
+	float x;
+	float y;
+	bool landed = alt < (alt0 + 0.1); // XXX improve?
+	map_projection_project(lat, lon, &x, &y); 
+	_localPos.timestamp = _pubTimeStamp;
+	_localPos.xy_valid = true;
+	_localPos.z_valid = true;
+	_localPos.v_xy_valid = true;
+	_localPos.v_z_valid = true;
+	_localPos.x = x;
+	_localPos.y = y;
+	_localPos.z = alt0 - alt;
+	_localPos.vx = vN;
+	_localPos.vy = vE;
+	_localPos.vz = vD;
+	_localPos.yaw = psi;
+	_localPos.xy_global = true;
+	_localPos.z_global = true;
+	_localPos.ref_timestamp = _pubTimeStamp;
+	_localPos.ref_lat = getLatDegE7();
+	_localPos.ref_lon = getLonDegE7();
+	_localPos.ref_alt = 0;
+	_localPos.landed = landed;
+
 	// attitude publication
 	_att.timestamp = _pubTimeStamp;
 	_att.roll = phi;
@@ -347,8 +378,10 @@ void KalmanNav::updatePublications()
 
 	// selectively update publications,
 	// do NOT call superblock do-all method
-	if (_positionInitialized)
+	if (_positionInitialized) {
 		_pos.update();
+		_localPos.update();
+	}
 
 	if (_attitudeInitialized)
 		_att.update();
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
index 799fc2fb9..a69bde1a6 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
+++ b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp
@@ -52,6 +52,7 @@
 
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/parameter_update.h>
@@ -142,12 +143,8 @@ protected:
 	control::UOrbSubscription<parameter_update_s> _param_update;    /**< parameter update sub. */
 	// publications
 	control::UOrbPublication<vehicle_global_position_s> _pos;       /**< position pub. */
+	control::UOrbPublication<vehicle_local_position_s> _localPos;   /**< local 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 */
@@ -164,8 +161,10 @@ protected:
 	float phi, theta, psi;                  /**< 3-2-1 euler angles */
 	float vN, vE, vD;                       /**< navigation velocity, m/s */
 	double lat, lon;                   	/**< lat, lon radians */
-	float alt;                   		/**< altitude, meters */
 	// parameters
+	float alt;                   		/**< altitude, meters */
+	double lat0, lon0;                   	/**< reference latitude and longitude */
+	float alt0;                   		/**<  refeerence altitude (ground height) */
 	control::BlockParamFloat _vGyro;      /**< gyro process noise */
 	control::BlockParamFloat _vAccel;     /**< accelerometer process noise  */
 	control::BlockParamFloat _rMag;       /**< magnetometer measurement noise  */
diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp
index d65045d68..6dc19df41 100644
--- a/src/modules/fixedwing_backside/fixedwing.cpp
+++ b/src/modules/fixedwing_backside/fixedwing.cpp
@@ -156,7 +156,8 @@ void BlockMultiModeBacksideAutopilot::update()
 		_actuators.control[i] = 0.0f;
 
 	// only update guidance in auto mode
-	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) {	// TODO use vehicle_control_mode here?
+	if (_status.main_state == MAIN_STATE_AUTO) {
+		// TODO use vehicle_control_mode here?
 		// update guidance
 		_guide.update(_pos, _att, _posCmd.current, _lastPosCmd.current);
 	}
@@ -166,14 +167,11 @@ void BlockMultiModeBacksideAutopilot::update()
 	// the setpoint should update to loitering around this position
 
 	// handle autopilot modes
-	if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
-	    _status.navigation_state == NAVIGATION_STATE_STABILIZE) {	// TODO use vehicle_control_mode here?
+	if (_status.main_state == MAIN_STATE_AUTO) {
 
-		// update guidance
-		_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
+		// calculate velocity, XXX should be airspeed,
+		// but using ground speed for now for the purpose 
+		// of control we will limit the velocity feedback between
 		// the min/max velocity
 		float v = _vLimit.update(sqrtf(
 					_pos.vx * _pos.vx +
@@ -218,19 +216,22 @@ void BlockMultiModeBacksideAutopilot::update()
 		// a first binary release can be targeted.
 		// This is not a hack, but a design choice.
 
-		/* do not limit in HIL */
+		// do not limit in HIL
 		if (_status.hil_state != HIL_STATE_ON) {
 			/* limit to value of manual throttle */
 			_actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ?
 						     _actuators.control[CH_THR] : _manual.throttle;
 		}
 
-	} else if (_status.navigation_state == NAVIGATION_STATE_DIRECT) {	// TODO use vehicle_control_mode here?
+	} else if (_status.main_state == MAIN_STATE_MANUAL) {
 		_actuators.control[CH_AIL] = _manual.roll;
 		_actuators.control[CH_ELV] = _manual.pitch;
 		_actuators.control[CH_RDR] = _manual.yaw;
 		_actuators.control[CH_THR] = _manual.throttle;
-	} else if (_status.navigation_state == NAVIGATION_STATE_STABILIZE) {	// TODO use vehicle_control_mode here?
+
+	} else if (_status.main_state == MAIN_STATE_SEATBELT ||
+		_status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {
+
 		// calculate velocity, XXX should be airspeed, but using ground speed for now
 		// for the purpose of control we will limit the velocity feedback between
 		// the min/max velocity
diff --git a/src/modules/fixedwing_backside/fixedwing.hpp b/src/modules/fixedwing_backside/fixedwing.hpp
index 3876e4630..567efeb35 100644
--- a/src/modules/fixedwing_backside/fixedwing.hpp
+++ b/src/modules/fixedwing_backside/fixedwing.hpp
@@ -255,13 +255,13 @@ private:
 	BlockWaypointGuidance _guide;
 
 	// block params
-	BlockParam<float> _trimAil;
-	BlockParam<float> _trimElv;
-	BlockParam<float> _trimRdr;
-	BlockParam<float> _trimThr;
-	BlockParam<float> _trimV;
-	BlockParam<float> _vCmd;
-	BlockParam<float> _crMax;
+	BlockParamFloat _trimAil;
+	BlockParamFloat _trimElv;
+	BlockParamFloat _trimRdr;
+	BlockParamFloat _trimThr;
+	BlockParamFloat _trimV;
+	BlockParamFloat _vCmd;
+	BlockParamFloat _crMax;
 
 	struct pollfd _attPoll;
 	vehicle_global_position_set_triplet_s _lastPosCmd;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index c51a6de08..bfccd5fb0 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -72,6 +72,7 @@
 #include <systemlib/airspeed.h>
 #include <mavlink/mavlink_log.h>
 #include <commander/px4_custom_mode.h>
+#include <geo/geo.h>
 
 __BEGIN_DECLS
 
@@ -99,11 +100,13 @@ static struct vehicle_command_s vcmd;
 static struct offboard_control_setpoint_s offboard_control_sp;
 
 struct vehicle_global_position_s hil_global_pos;
+struct vehicle_local_position_s hil_local_pos;
 struct vehicle_attitude_s hil_attitude;
 struct vehicle_gps_position_s hil_gps;
 struct sensor_combined_s hil_sensors;
 struct battery_status_s	hil_battery_status;
 static orb_advert_t pub_hil_global_pos = -1;
+static orb_advert_t pub_hil_local_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;
@@ -126,6 +129,11 @@ static orb_advert_t offboard_control_sp_pub = -1;
 static orb_advert_t vicon_position_pub = -1;
 static orb_advert_t telemetry_status_pub = -1;
 
+// variables for HIL reference position
+static int32_t lat0 = 0;
+static int32_t lon0 = 0;
+static double alt0 = 0;
+
 static void
 handle_message(mavlink_message_t *msg)
 {
@@ -621,24 +629,61 @@ handle_message(mavlink_message_t *msg)
 				orb_publish(ORB_ID(airspeed), pub_hil_airspeed, &airspeed);
 			}
 
-			hil_global_pos.valid = true;
-			hil_global_pos.lat = hil_state.lat;
-			hil_global_pos.lon = hil_state.lon;
-			hil_global_pos.alt = hil_state.alt / 1000.0f;
-			hil_global_pos.vx = hil_state.vx / 100.0f;
-			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();
+			uint64_t timestamp = hrt_absolute_time();
 
+			// publish global position	
 			if (pub_hil_global_pos > 0) {
 				orb_publish(ORB_ID(vehicle_global_position), pub_hil_global_pos, &hil_global_pos);
+				// global position packet
+				hil_global_pos.timestamp = timestamp;
+				hil_global_pos.valid = true;
+				hil_global_pos.lat = hil_state.lat;
+				hil_global_pos.lon = hil_state.lon;
+				hil_global_pos.alt = hil_state.alt / 1000.0f;
+				hil_global_pos.vx = hil_state.vx / 100.0f;
+				hil_global_pos.vy = hil_state.vy / 100.0f;
+				hil_global_pos.vz = hil_state.vz / 100.0f;
 
 			} else {
 				pub_hil_global_pos = orb_advertise(ORB_ID(vehicle_global_position), &hil_global_pos);
 			}
 
+			// publish local position
+			if (pub_hil_local_pos > 0) {
+				float x;
+				float y;
+				bool landed = hil_state.alt/1000.0f < (alt0 + 0.1); // XXX improve?
+				double lat = hil_state.lat*1e-7;
+				double lon = hil_state.lon*1e-7;
+				map_projection_project(lat, lon, &x, &y); 
+				hil_local_pos.timestamp = timestamp;
+				hil_local_pos.xy_valid = true;
+				hil_local_pos.z_valid = true;
+				hil_local_pos.v_xy_valid = true;
+				hil_local_pos.v_z_valid = true;
+				hil_local_pos.x = x;
+				hil_local_pos.y = y;
+				hil_local_pos.z = alt0 - hil_state.alt/1000.0f;
+				hil_local_pos.vx = hil_state.vx/100.0f;
+				hil_local_pos.vy = hil_state.vy/100.0f;
+				hil_local_pos.vz = hil_state.vz/100.0f;
+				hil_local_pos.yaw = hil_attitude.yaw;
+				hil_local_pos.xy_global = true;
+				hil_local_pos.z_global = true;
+				hil_local_pos.ref_timestamp = timestamp;
+				hil_local_pos.ref_lat = hil_state.lat;
+				hil_local_pos.ref_lon = hil_state.lon;
+				hil_local_pos.ref_alt = alt0;
+				hil_local_pos.landed = landed;
+				orb_publish(ORB_ID(vehicle_local_position), pub_hil_local_pos, &hil_local_pos);
+			} else {
+				pub_hil_local_pos = orb_advertise(ORB_ID(vehicle_local_position), &hil_local_pos);
+				lat0 = hil_state.lat;
+				lon0 = hil_state.lon;
+				alt0 = hil_state.alt / 1000.0f;
+				map_projection_init(hil_state.lat, hil_state.lon);
+			}
+
 			/* Calculate Rotation Matrix */
 			math::Quaternion q(hil_state.attitude_quaternion);
 			math::Dcm C_nb(q);
diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c
index f6860930c..abc91d34f 100644
--- a/src/modules/mavlink/orb_listener.c
+++ b/src/modules/mavlink/orb_listener.c
@@ -75,6 +75,7 @@ struct actuator_armed_s armed;
 struct actuator_controls_effective_s actuators_effective_0;
 struct actuator_controls_s actuators_0;
 struct vehicle_attitude_s att;
+struct airspeed_s airspeed;
 
 struct mavlink_subscriptions mavlink_subs;
 
@@ -92,6 +93,8 @@ static unsigned int gps_counter;
  */
 static uint64_t last_sensor_timestamp;
 
+static hrt_abstime last_sent_vfr = 0;
+
 static void	*uorb_receive_thread(void *arg);
 
 struct listener {
@@ -229,7 +232,7 @@ l_vehicle_attitude(const struct listener *l)
 	/* copy attitude data into local buffer */
 	orb_copy(ORB_ID(vehicle_attitude), mavlink_subs.att_sub, &att);
 
-	if (gcs_link)
+	if (gcs_link) {
 		/* send sensor values */
 		mavlink_msg_attitude_send(MAVLINK_COMM_0,
 					  last_sensor_timestamp / 1000,
@@ -240,6 +243,17 @@ l_vehicle_attitude(const struct listener *l)
 					  att.pitchspeed,
 					  att.yawspeed);
 
+		/* limit VFR message rate to 10Hz */
+		hrt_abstime t = hrt_absolute_time();
+		if (t >= last_sent_vfr + 100000) {
+			last_sent_vfr = t;
+			float groundspeed = sqrtf(global_pos.vx * global_pos.vx + global_pos.vy * global_pos.vy);
+			uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
+			float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
+			mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, global_pos.alt, -global_pos.vz);
+		}
+	}
+
 	attitude_counter++;
 }
 
@@ -681,17 +695,7 @@ l_home(const struct listener *l)
 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);
-	uint16_t heading = (att.yaw + M_PI_F) / M_PI_F * 180.0f;
-	float throttle = actuators_effective_0.control_effective[3] * (UINT16_MAX - 1);
-	float alt = global_pos.relative_alt;
-	float climb = -global_pos.vz;
-
-	mavlink_msg_vfr_hud_send(MAVLINK_COMM_0, airspeed.true_airspeed_m_s, groundspeed, heading, throttle, alt, climb);
 }
 
 void
@@ -849,7 +853,7 @@ 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 --- */
+	/* --- AIRSPEED --- */
 	mavlink_subs.airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	orb_set_interval(mavlink_subs.airspeed_sub, 200); 	/* 5Hz updates */
 
diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c
index adb63186c..86ac0e4ff 100644
--- a/src/modules/multirotor_att_control/multirotor_rate_control.c
+++ b/src/modules/multirotor_att_control/multirotor_rate_control.c
@@ -59,31 +59,23 @@
 #include <systemlib/err.h>
 #include <drivers/drv_hrt.h>
 
-PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); /* same on Flamewheel */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.005f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.2f);
-//PARAM_DEFINE_FLOAT(MC_YAWRATE_AWU, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_YAWRATE_LIM, 1.0f);
 
-PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f); /* 0.15 F405 Flamewheel */
+PARAM_DEFINE_FLOAT(MC_ATTRATE_P, 0.09f);
 PARAM_DEFINE_FLOAT(MC_ATTRATE_D, 0.002f);
 PARAM_DEFINE_FLOAT(MC_ATTRATE_I, 0.0f);
-//PARAM_DEFINE_FLOAT(MC_ATTRATE_AWU, 0.05f);
-//PARAM_DEFINE_FLOAT(MC_ATTRATE_LIM, 1.0f);	/**< roughly < 500 deg/s limit */
 
 struct mc_rate_control_params {
 
 	float yawrate_p;
 	float yawrate_d;
 	float yawrate_i;
-	//float yawrate_awu;
-	//float yawrate_lim;
 
 	float attrate_p;
 	float attrate_d;
 	float attrate_i;
-	//float attrate_awu;
-	//float attrate_lim;
 
 	float rate_lim;
 };
@@ -93,14 +85,10 @@ struct mc_rate_control_param_handles {
 	param_t yawrate_p;
 	param_t yawrate_i;
 	param_t yawrate_d;
-	//param_t yawrate_awu;
-	//param_t yawrate_lim;
 
 	param_t attrate_p;
 	param_t attrate_i;
 	param_t attrate_d;
-	//param_t attrate_awu;
-	//param_t attrate_lim;
 };
 
 /**
@@ -122,14 +110,10 @@ static int parameters_init(struct mc_rate_control_param_handles *h)
 	h->yawrate_p 	=	param_find("MC_YAWRATE_P");
 	h->yawrate_i 	=	param_find("MC_YAWRATE_I");
 	h->yawrate_d 	=	param_find("MC_YAWRATE_D");
-	//h->yawrate_awu 	=	param_find("MC_YAWRATE_AWU");
-	//h->yawrate_lim 	=	param_find("MC_YAWRATE_LIM");
 
 	h->attrate_p 	= 	param_find("MC_ATTRATE_P");
 	h->attrate_i 	= 	param_find("MC_ATTRATE_I");
 	h->attrate_d 	= 	param_find("MC_ATTRATE_D");
-	//h->attrate_awu 	= 	param_find("MC_ATTRATE_AWU");
-	//h->attrate_lim 	= 	param_find("MC_ATTRATE_LIM");
 
 	return OK;
 }
@@ -139,14 +123,10 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru
 	param_get(h->yawrate_p, &(p->yawrate_p));
 	param_get(h->yawrate_i, &(p->yawrate_i));
 	param_get(h->yawrate_d, &(p->yawrate_d));
-	//param_get(h->yawrate_awu, &(p->yawrate_awu));
-	//param_get(h->yawrate_lim, &(p->yawrate_lim));
 
 	param_get(h->attrate_p, &(p->attrate_p));
 	param_get(h->attrate_i, &(p->attrate_i));
 	param_get(h->attrate_d, &(p->attrate_d));
-	//param_get(h->attrate_awu, &(p->attrate_awu));
-	//param_get(h->attrate_lim, &(p->attrate_lim));
 
 	return OK;
 }
@@ -168,6 +148,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 	static PID_t pitch_rate_controller;
 	static PID_t roll_rate_controller;
+	static PID_t yaw_rate_controller;
 
 	static struct mc_rate_control_params p;
 	static struct mc_rate_control_param_handles h;
@@ -182,7 +163,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 
 		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);
-
+		pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f, PID_MODE_DERIVATIV_CALC_NO_SP, 0.003f);
 	}
 
 	/* load new parameters with lower rate */
@@ -191,35 +172,24 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp,
 		parameters_update(&h, &p);
 		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);
+		pid_set_parameters(&yaw_rate_controller,  p.yawrate_p, p.yawrate_i, p.yawrate_d, 1.0f, 1.0f);
 	}
 
 	/* reset integrals if needed */
 	if (reset_integral) {
 		pid_reset_integral(&pitch_rate_controller);
 		pid_reset_integral(&roll_rate_controller);
-		// TODO pid_reset_integral(&yaw_rate_controller);
+		pid_reset_integral(&yaw_rate_controller);
 	}
 
-	/* control pitch (forward) output */
-	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch ,
-					    rates[1], 0.0f, deltaT);
-
-	/* control roll (left/right) output */
-	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll ,
-					   rates[0], 0.0f, deltaT);
-
-	/* 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 */
-	if (!isfinite(yaw_rate_control)) {
-		yaw_rate_control = 0.0f;
-		warnx("rej. NaN ctrl yaw");
-	}
+	/* run pitch, roll and yaw controllers */
+	float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT);
+	float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT);
+	float yaw_control = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT);
 
 	actuators->control[0] = roll_control;
 	actuators->control[1] = pitch_control;
-	actuators->control[2] = yaw_rate_control;
+	actuators->control[2] = yaw_control;
 	actuators->control[3] = rate_sp->thrust;
 
 	motor_skip_counter++;
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 36dd370fb..3d23d0c09 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -471,7 +471,10 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 							} else {
 								local_pos_sp.z = local_pos.ref_alt - global_pos_sp.altitude;
 							}
-							att_sp.yaw_body = global_pos_sp.yaw;
+							/* update yaw setpoint only if value is valid */
+							if (isfinite(global_pos_sp.yaw) && fabsf(global_pos_sp.yaw) < M_TWOPI) {
+								att_sp.yaw_body = global_pos_sp.yaw;
+							}
 
 							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);