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

Conflicts:
	src/modules/mavlink/mavlink_main.cpp

56 files changed, 1995 insertions, 533 deletions

diff --git a/src/drivers/airspeed/module.mk b/src/drivers/airspeed/module.mk
index 4eef06161..5fbc75309 100644
--- a/src/drivers/airspeed/module.mk
+++ b/src/drivers/airspeed/module.mk
@@ -36,3 +36,5 @@
 #
 
 SRCS			= airspeed.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/bma180/module.mk b/src/drivers/bma180/module.mk
index 4c60ee082..33433307a 100644
--- a/src/drivers/bma180/module.mk
+++ b/src/drivers/bma180/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= bma180
 
 SRCS		= bma180.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index 972573f9f..84815fdfb 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -202,6 +202,9 @@ ORB_DECLARE(output_pwm);
 /** force safety switch off (to disable use of safety switch) */
 #define PWM_SERVO_SET_FORCE_SAFETY_OFF _IOC(_PWM_SERVO_BASE, 23)
 
+/** force failsafe mode (failsafe values are set immediately even if failsafe condition not met) */
+#define PWM_SERVO_SET_FORCE_FAILSAFE _IOC(_PWM_SERVO_BASE, 24)
+
 /*
  *
  *
diff --git a/src/drivers/ets_airspeed/module.mk b/src/drivers/ets_airspeed/module.mk
index 966a5b819..8aaaf0ebb 100644
--- a/src/drivers/ets_airspeed/module.mk
+++ b/src/drivers/ets_airspeed/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND		= ets_airspeed
 SRCS			= ets_airspeed.cpp
 
 MODULE_STACKSIZE	= 1200
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/hil/module.mk b/src/drivers/hil/module.mk
index f8895f5d5..f1fc49fb3 100644
--- a/src/drivers/hil/module.mk
+++ b/src/drivers/hil/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= hil
 
 SRCS		= hil.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/led/module.mk b/src/drivers/led/module.mk
index 777f3e442..5b7b4491b 100644
--- a/src/drivers/led/module.mk
+++ b/src/drivers/led/module.mk
@@ -36,3 +36,5 @@
 #
 
 SRCS		= led.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/md25/module.mk b/src/drivers/md25/module.mk
index 13821a6b5..3f9cf2d89 100644
--- a/src/drivers/md25/module.mk
+++ b/src/drivers/md25/module.mk
@@ -40,3 +40,5 @@ MODULE_COMMAND		= md25
 
 SRCS			= md25.cpp \
 			  md25_main.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/meas_airspeed/module.mk b/src/drivers/meas_airspeed/module.mk
index 2a15b669f..6f5909978 100644
--- a/src/drivers/meas_airspeed/module.mk
+++ b/src/drivers/meas_airspeed/module.mk
@@ -42,3 +42,5 @@ SRCS			= meas_airspeed.cpp
 MODULE_STACKSIZE	= 1200
 
 EXTRACXXFLAGS	= -Weffc++
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/ms5611/module.mk b/src/drivers/ms5611/module.mk
index 20f8aa173..ee74058fc 100644
--- a/src/drivers/ms5611/module.mk
+++ b/src/drivers/ms5611/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= ms5611
 
 SRCS		= ms5611.cpp ms5611_spi.cpp ms5611_i2c.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/px4flow/module.mk b/src/drivers/px4flow/module.mk
index d3062e457..460bec7b9 100644
--- a/src/drivers/px4flow/module.mk
+++ b/src/drivers/px4flow/module.mk
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= px4flow
 
 SRCS		= px4flow.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 711674886..d93009c47 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1149,6 +1149,12 @@ PX4IO::io_set_arming_state()
 		clear |= PX4IO_P_SETUP_ARMING_LOCKDOWN;
 	}
 
+	if (armed.force_failsafe) {
+		set |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
+	} else {
+		clear |= PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE;
+	}
+
 	if (armed.ready_to_arm) {
 		set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK;
 
@@ -2002,7 +2008,7 @@ PX4IO::print_status(bool extended_status)
 		((features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) ? " RSSI_ADC" : "")
 		);
 	uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING);
-	printf("arming 0x%04x%s%s%s%s%s%s%s\n",
+	printf("arming 0x%04x%s%s%s%s%s%s%s%s\n",
 	       arming,
 	       ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)		? " FMU_ARMED" : " FMU_DISARMED"),
 	       ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)		? " IO_ARM_OK" : " IO_ARM_DENIED"),
@@ -2010,7 +2016,9 @@ PX4IO::print_status(bool extended_status)
 	       ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)		? " FAILSAFE_CUSTOM" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK)	? " INAIR_RESTART_OK" : ""),
 	       ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE)	? " ALWAYS_PWM_ENABLE" : ""),
-	       ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)		? " LOCKDOWN" : ""));
+	       ((arming & PX4IO_P_SETUP_ARMING_LOCKDOWN)		? " LOCKDOWN" : ""),
+	       ((arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)		? " FORCE_FAILSAFE" : "")
+	       );
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 	printf("rates 0x%04x default %u alt %u relays 0x%04x\n",
 	       io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES),
@@ -2222,6 +2230,17 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FORCE_SAFETY_OFF, PX4IO_FORCE_SAFETY_MAGIC);
 		break;
 
+	case PWM_SERVO_SET_FORCE_FAILSAFE:
+		/* force failsafe mode instantly */
+		if (arg == 0) {
+			/* clear force failsafe flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE, 0);
+		} else {
+			/* set force failsafe flag */
+			ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, 0, PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE);
+		}
+		break;
+
 	case DSM_BIND_START:
 
 		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
@@ -2424,7 +2443,7 @@ PX4IO::ioctl(file * filep, int cmd, unsigned long arg)
 		break;
 
 	case PX4IO_CHECK_CRC: {
-		/* check IO firmware CRC against passed value */		
+		/* check IO firmware CRC against passed value */
 		uint32_t io_crc = 0;
 		ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_CRC, (uint16_t *)&io_crc, 2);
 		if (ret != OK)
@@ -2684,7 +2703,7 @@ checkcrc(int argc, char *argv[])
 	int fd = open(argv[1], O_RDONLY);
 	if (fd == -1) {
 		printf("open of %s failed - %d\n", argv[1], errno);
-		exit(1);			
+		exit(1);
 	}
 	const uint32_t app_size_max = 0xf000;
 	uint32_t fw_crc = 0;
@@ -2699,7 +2718,7 @@ checkcrc(int argc, char *argv[])
 	close(fd);
 	while (nbytes < app_size_max) {
 		uint8_t b = 0xff;
-		fw_crc = crc32part(&b, 1, fw_crc);			
+		fw_crc = crc32part(&b, 1, fw_crc);
 		nbytes++;
 	}
 
@@ -2712,7 +2731,7 @@ checkcrc(int argc, char *argv[])
 
 	if (ret != OK) {
 		printf("check CRC failed - %d\n", ret);
-		exit(1);			
+		exit(1);
 	}
 	printf("CRCs match\n");
 	exit(0);
@@ -2742,12 +2761,12 @@ bind(int argc, char *argv[])
 		pulses = DSMX_BIND_PULSES;
 	else if (!strcmp(argv[2], "dsmx8"))
 		pulses = DSMX8_BIND_PULSES;
-	else 
+	else
 		errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]);
 	// Test for custom pulse parameter
 	if (argc > 3)
 		pulses = atoi(argv[3]);
-	if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) 
+	if (g_dev->system_status() & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)
 		errx(1, "system must not be armed");
 
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
@@ -2949,7 +2968,7 @@ lockdown(int argc, char *argv[])
 				(void)g_dev->ioctl(0, PWM_SERVO_SET_DISABLE_LOCKDOWN, 0);
 				warnx("ACTUATORS ARE NOW SAFE IN HIL.");
 			}
-			
+
 		} else {
 			errx(1, "driver not loaded, exiting");
 		}
diff --git a/src/drivers/roboclaw/module.mk b/src/drivers/roboclaw/module.mk
index 1abecf198..c5e55bdc3 100644
--- a/src/drivers/roboclaw/module.mk
+++ b/src/drivers/roboclaw/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND		= roboclaw
 
 SRCS			= roboclaw_main.cpp \
 			  RoboClaw.cpp
+
+MAXOPTIMIZATION	 = -Os
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index bdeb0b3d4..eac8b0afc 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -546,24 +546,19 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 		}
 		break;
 
-#if 0
 	/* Flight termination */
-	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command
-
-			//XXX: to enable the parachute, a param needs to be set
-			//xxx: for safety only for now, param3 is unused by VEHICLE_CMD_DO_SET_SERVO
-			if (armed_local->armed && cmd->param3 > 0.5 && parachute_enabled) {
-				transition_result_t failsafe_res = failsafe_state_transition(status, FAILSAFE_STATE_TERMINATION);
-				cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
-
+	case VEHICLE_CMD_DO_FLIGHTTERMINATION: {
+			if (cmd->param1 > 0.5f) {
+				//XXX update state machine?
+				armed_local->force_failsafe = true;
+				warnx("forcing failsafe");
 			} else {
-				/* reject parachute depoyment not armed */
-				cmd_result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+				armed_local->force_failsafe = false;
+				warnx("disabling failsafe");
 			}
-
+			cmd_result = VEHICLE_CMD_RESULT_ACCEPTED;
 		}
 		break;
-#endif
 
 	case VEHICLE_CMD_DO_SET_HOME: {
 			bool use_current = cmd->param1 > 0.5f;
@@ -940,6 +935,11 @@ int commander_thread_main(int argc, char *argv[])
 	struct system_power_s system_power;
 	memset(&system_power, 0, sizeof(system_power));
 
+	/* Subscribe to actuator controls (outputs) */
+	int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
+	struct actuator_controls_s actuator_controls;
+	memset(&actuator_controls, 0, sizeof(actuator_controls));
+
 	control_status_leds(&status, &armed, true);
 
 	/* now initialized */
@@ -1221,13 +1221,17 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
+			orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
 			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
-				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah);
+
+				/* get throttle (if armed), as we only care about energy negative throttle also counts */
+				float throttle = (armed.armed) ? fabsf(actuator_controls.control[3]) : 0.0f;
+				status.battery_remaining = battery_remaining_estimate_voltage(battery.voltage_filtered_v, battery.discharged_mah, throttle);
 			}
 		}
 
@@ -1289,13 +1293,13 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* if battery voltage is getting lower, warn using buzzer, etc. */
-		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.25f && !low_battery_voltage_actions_done) {
+		if (status.condition_battery_voltage_valid && status.battery_remaining < 0.18f && !low_battery_voltage_actions_done) {
 			low_battery_voltage_actions_done = true;
 			mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
 
-		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
+		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.09f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
 			mavlink_log_emergency(mavlink_fd, "CRITICAL BATTERY, LAND IMMEDIATELY");
@@ -1434,8 +1438,12 @@ int commander_thread_main(int argc, char *argv[])
 				arming_state_changed = true;
 
 			} else if (arming_ret == TRANSITION_DENIED) {
-				/* DENIED here indicates bug in the commander */
-				mavlink_log_critical(mavlink_fd, "arming state transition denied");
+				/*
+				 * the arming transition can be denied to a number of reasons:
+				 *  - pre-flight check failed (sensors not ok or not calibrated)
+				 *  - safety not disabled
+				 *  - system not in manual mode
+				 */
 				tune_negative(true);
 			}
 
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index d5fe122cb..2022e99fb 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -281,15 +281,17 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern)
 	}
 }
 
-float battery_remaining_estimate_voltage(float voltage, float discharged)
+float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized)
 {
 	float ret = 0;
 	static param_t bat_v_empty_h;
 	static param_t bat_v_full_h;
 	static param_t bat_n_cells_h;
 	static param_t bat_capacity_h;
-	static float bat_v_empty = 3.2f;
-	static float bat_v_full = 4.0f;
+	static param_t bat_v_load_drop_h;
+	static float bat_v_empty = 3.4f;
+	static float bat_v_full = 4.2f;
+	static float bat_v_load_drop = 0.06f;
 	static int bat_n_cells = 3;
 	static float bat_capacity = -1.0f;
 	static bool initialized = false;
@@ -297,23 +299,26 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
 
 	if (!initialized) {
 		bat_v_empty_h = param_find("BAT_V_EMPTY");
-		bat_v_full_h = param_find("BAT_V_FULL");
+		bat_v_full_h = param_find("BAT_V_CHARGED");
 		bat_n_cells_h = param_find("BAT_N_CELLS");
 		bat_capacity_h = param_find("BAT_CAPACITY");
+		bat_v_load_drop_h = param_find("BAT_V_LOAD_DROP");
 		initialized = true;
 	}
 
 	if (counter % 100 == 0) {
 		param_get(bat_v_empty_h, &bat_v_empty);
 		param_get(bat_v_full_h, &bat_v_full);
+		param_get(bat_v_load_drop_h, &bat_v_load_drop);
 		param_get(bat_n_cells_h, &bat_n_cells);
 		param_get(bat_capacity_h, &bat_capacity);
 	}
 
 	counter++;
 
-	/* remaining charge estimate based on voltage */
-	float remaining_voltage = (voltage - bat_n_cells * bat_v_empty) / (bat_n_cells * (bat_v_full - bat_v_empty));
+	/* remaining charge estimate based on voltage and internal resistance (drop under load) */
+	float bat_v_full_dynamic = bat_v_full - (bat_v_load_drop * throttle_normalized);
+	float remaining_voltage = (voltage - (bat_n_cells * bat_v_empty)) / (bat_n_cells * (bat_v_full_dynamic - bat_v_empty));
 
 	if (bat_capacity > 0.0f) {
 		/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h
index a49c9e263..4a77fe487 100644
--- a/src/modules/commander/commander_helper.h
+++ b/src/modules/commander/commander_helper.h
@@ -80,8 +80,9 @@ void rgbled_set_pattern(rgbled_pattern_t *pattern);
  *
  * @param voltage the current battery voltage
  * @param discharged the discharged capacity
+ * @param throttle_normalized the normalized throttle magnitude from 0 to 1. Negative throttle should be converted to this range as well, as it consumes energy.
  * @return the estimated remaining capacity in 0..1
  */
-float battery_remaining_estimate_voltage(float voltage, float discharged);
+float battery_remaining_estimate_voltage(float voltage, float discharged, float throttle_normalized);
 
 #endif /* COMMANDER_HELPER_H_ */
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 4750f9d5c..dba68700b 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -65,7 +65,17 @@ PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.4f);
  *
  * @group Battery Calibration
  */
-PARAM_DEFINE_FLOAT(BAT_V_FULL, 3.9f);
+PARAM_DEFINE_FLOAT(BAT_V_CHARGED, 4.2f);
+
+/**
+ * Voltage drop per cell on 100% load
+ *
+ * This implicitely defines the internal resistance
+ * to maximum current ratio and assumes linearity.
+ *
+ * @group Battery Calibration
+ */
+PARAM_DEFINE_FLOAT(BAT_V_LOAD_DROP, 0.07f);
 
 /**
  * Number of cells.
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 372ba9d7d..7b26e3e8c 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -110,8 +110,8 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
 
 	transition_result_t ret = TRANSITION_DENIED;
-
 	arming_state_t current_arming_state = status->arming_state;
+	bool feedback_provided = false;
 
 	/* only check transition if the new state is actually different from the current one */
 	if (new_arming_state == current_arming_state) {
@@ -156,13 +156,15 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 
 					// Fail transition if pre-arm check fails
 					if (prearm_ret) {
+						/* the prearm check already prints the reject reason */
+						feedback_provided = true;
 						valid_transition = false;
 
 					// Fail transition if we need safety switch press
 					} else if (safety->safety_switch_available && !safety->safety_off) {
 
-						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch!");
-
+						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first!");
+						feedback_provided = true;
 						valid_transition = false;
 					}
 
@@ -173,6 +175,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 						if (!status->condition_power_input_valid) {
 
 							mavlink_log_critical(mavlink_fd, "NOT ARMING: Connect power module.");
+							feedback_provided = true;
 							valid_transition = false;
 						}
 
@@ -182,6 +185,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 							(status->avionics_power_rail_voltage < 4.9f))) {
 
 							mavlink_log_critical(mavlink_fd, "NOT ARMING: Avionics power low: %6.2f V.", (double)status->avionics_power_rail_voltage);
+							feedback_provided = true;
 							valid_transition = false;
 						}
 					}
@@ -200,6 +204,8 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 
 		/* Sensors need to be initialized for STANDBY state */
 		if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+			mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
+			feedback_provided = true;
 			valid_transition = false;
 		}
 
@@ -216,11 +222,14 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	}
 
 	if (ret == TRANSITION_DENIED) {
-		static const char *errMsg = "INVAL: %s - %s";
+		const char * str = "INVAL: %s - %s";
+		/* only print to console here by default as this is too technical to be useful during operation */
+		warnx(str, state_names[status->arming_state], state_names[new_arming_state]);
 
-		mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
-
-		warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+		/* print to MAVLink if we didn't provide any feedback yet */
+		if (!feedback_provided) {
+			mavlink_log_critical(mavlink_fd, str, state_names[status->arming_state], state_names[new_arming_state]);
+		}
 	}
 
 	return ret;
@@ -648,8 +657,7 @@ int prearm_check(const struct vehicle_status_s *status, const int mavlink_fd)
 		float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
 
 		if (accel_magnitude < 4.0f || accel_magnitude > 15.0f /* m/s^2 */) {
-			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE");
-			mavlink_log_critical(mavlink_fd, "hold still while arming");
+			mavlink_log_critical(mavlink_fd, "ARM FAIL: ACCEL RANGE, hold still");
 			/* this is frickin' fatal */
 			failed = true;
 			goto system_eval;
diff --git a/src/modules/controllib/block/Block.hpp b/src/modules/controllib/block/Block.hpp
index 736698e21..9bd80b15b 100644
--- a/src/modules/controllib/block/Block.hpp
+++ b/src/modules/controllib/block/Block.hpp
@@ -93,6 +93,11 @@ protected:
 	List<uORB::SubscriptionBase *> _subscriptions;
 	List<uORB::PublicationBase *> _publications;
 	List<BlockParamBase *> _params;
+
+private:
+	/* this class has pointer data members and should not be copied (private constructor) */
+	Block(const control::Block&);
+	Block operator=(const control::Block&);
 };
 
 class __EXPORT SuperBlock :
diff --git a/src/modules/controllib/blocks.cpp b/src/modules/controllib/blocks.cpp
index c6c374300..0175acda9 100644
--- a/src/modules/controllib/blocks.cpp
+++ b/src/modules/controllib/blocks.cpp
@@ -293,7 +293,18 @@ int blockIntegralTrapTest()
 
 float BlockDerivative::update(float input)
 {
-	float output = _lowPass.update((input - getU()) / getDt());
+	float output;
+	if (_initialized) {
+		output = _lowPass.update((input - getU()) / getDt());
+	} else {
+		// if this is the first call to update
+		// we have no valid derivative
+		// and so we use the assumption the
+		// input value is not changing much,
+		// which is the best we can do here.
+		output = 0.0f;
+		_initialized = true;
+	}
 	setU(input);
 	return output;
 }
diff --git a/src/modules/controllib/blocks.hpp b/src/modules/controllib/blocks.hpp
index 66e929038..37d7832b3 100644
--- a/src/modules/controllib/blocks.hpp
+++ b/src/modules/controllib/blocks.hpp
@@ -238,9 +238,25 @@ public:
 	BlockDerivative(SuperBlock *parent, const char *name) :
 		SuperBlock(parent, name),
 		_u(0),
+		_initialized(false),
 		_lowPass(this, "LP")
 	{};
 	virtual ~BlockDerivative() {};
+
+	/**
+	 * Update the state and get current derivative
+	 *
+	 * This call updates the state and gets the current
+	 * derivative. As the derivative is only valid
+	 * on the second call to update, it will return
+	 * no change (0) on the first. To get a closer
+	 * estimate of the derivative on the first call,
+	 * call setU() one time step before using the
+	 * return value of update().
+	 *
+	 * @param input the variable to calculate the derivative of
+	 * @return the current derivative
+	 */
 	float update(float input);
 // accessors
 	void setU(float u) {_u = u;}
@@ -249,6 +265,7 @@ public:
 protected:
 // attributes
 	float _u; /**< previous input */
+	bool _initialized;
 	BlockLowPass _lowPass; /**< low pass filter */
 };
 
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 08c996ebc..0b111f7bd 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -136,7 +136,6 @@ private:
 	int		_global_pos_sub;
 	int		_pos_sp_triplet_sub;
 	int		_att_sub;			/**< vehicle attitude subscription */
-	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
 	int		_control_mode_sub;		/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
@@ -160,18 +159,6 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	/** manual control states */
-	float		_altctrl_hold_heading;		/**< heading the system should hold in altctrl mode */
-	double		_loiter_hold_lat;
-	double		_loiter_hold_lon;
-	float		_loiter_hold_alt;
-	bool		_loiter_hold;
-
-	double		_launch_lat;
-	double		_launch_lon;
-	float		_launch_alt;
-	bool		_launch_valid;
-
 	/* land states */
 	/* not in non-abort mode for landing yet */
 	bool land_noreturn_horizontal;
@@ -188,8 +175,8 @@ private:
 
 	/* Landingslope object */
 	Landingslope landingslope;
-
 	float flare_curve_alt_rel_last;
+
 	/* heading hold */
 	float target_bearing;
 
@@ -212,19 +199,6 @@ private:
 		float l1_period;
 		float l1_damping;
 
-		float time_const;
-		float min_sink_rate;
-		float max_sink_rate;
-		float max_climb_rate;
-		float throttle_damp;
-		float integrator_gain;
-		float vertical_accel_limit;
-		float height_comp_filter_omega;
-		float speed_comp_filter_omega;
-		float roll_throttle_compensation;
-		float speed_weight;
-		float pitch_damping;
-
 		float airspeed_min;
 		float airspeed_trim;
 		float airspeed_max;
@@ -238,9 +212,6 @@ private:
 
 		float throttle_land_max;
 
-		float heightrate_p;
-		float speedrate_p;
-
 		float land_slope_angle;
 		float land_H1_virt;
 		float land_flare_alt_relative;
@@ -255,19 +226,6 @@ private:
 		param_t l1_period;
 		param_t l1_damping;
 
-		param_t time_const;
-		param_t min_sink_rate;
-		param_t max_sink_rate;
-		param_t max_climb_rate;
-		param_t throttle_damp;
-		param_t integrator_gain;
-		param_t vertical_accel_limit;
-		param_t height_comp_filter_omega;
-		param_t speed_comp_filter_omega;
-		param_t roll_throttle_compensation;
-		param_t speed_weight;
-		param_t pitch_damping;
-
 		param_t airspeed_min;
 		param_t airspeed_trim;
 		param_t airspeed_max;
@@ -281,9 +239,6 @@ private:
 
 		param_t throttle_land_max;
 
-		param_t heightrate_p;
-		param_t speedrate_p;
-
 		param_t land_slope_angle;
 		param_t land_H1_virt;
 		param_t land_flare_alt_relative;
@@ -416,12 +371,14 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_control_mode_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
+	_sensor_combined_sub(-1),
 	_range_finder_sub(-1),
 
 /* publications */
 	_attitude_sp_pub(-1),
 	_nav_capabilities_pub(-1),
 
+/* states */
 	_att(),
 	_att_sp(),
 	_nav_capabilities(),
@@ -436,8 +393,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
 
-/* states */
-	_loiter_hold(false),
 	land_noreturn_horizontal(false),
 	land_noreturn_vertical(false),
 	land_stayonground(false),
@@ -446,12 +401,16 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	launch_detected(false),
 	usePreTakeoffThrust(false),
 	last_manual(false),
+	landingslope(),
 	flare_curve_alt_rel_last(0.0f),
+	target_bearing(0.0f),
 	launchDetector(),
 	_airspeed_error(0.0f),
 	_airspeed_valid(false),
+	_airspeed_last_valid(0),
 	_groundspeed_undershoot(0.0f),
 	_global_pos_valid(false),
+	_l1_control(),
 	_mTecs(),
 	_was_pos_control_mode(false)
 {
@@ -479,21 +438,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
 	_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
 
-	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
-	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
-	_parameter_handles.max_sink_rate =			param_find("FW_T_SINK_MAX");
-	_parameter_handles.max_climb_rate =			param_find("FW_T_CLMB_MAX");
-	_parameter_handles.throttle_damp = 			param_find("FW_T_THR_DAMP");
-	_parameter_handles.integrator_gain =			param_find("FW_T_INTEG_GAIN");
-	_parameter_handles.vertical_accel_limit =		param_find("FW_T_VERT_ACC");
-	_parameter_handles.height_comp_filter_omega =		param_find("FW_T_HGT_OMEGA");
-	_parameter_handles.speed_comp_filter_omega =		param_find("FW_T_SPD_OMEGA");
-	_parameter_handles.roll_throttle_compensation = 	param_find("FW_T_RLL2THR");
-	_parameter_handles.speed_weight = 			param_find("FW_T_SPDWEIGHT");
-	_parameter_handles.pitch_damping = 			param_find("FW_T_PTCH_DAMP");
-	_parameter_handles.heightrate_p =			param_find("FW_T_HRATE_P");
-	_parameter_handles.speedrate_p =			param_find("FW_T_SRATE_P");
-
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -544,22 +488,6 @@ FixedwingPositionControl::parameters_update()
 
 	param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
 
-	param_get(_parameter_handles.time_const, &(_parameters.time_const));
-	param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
-	param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
-	param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp));
-	param_get(_parameter_handles.integrator_gain, &(_parameters.integrator_gain));
-	param_get(_parameter_handles.vertical_accel_limit, &(_parameters.vertical_accel_limit));
-	param_get(_parameter_handles.height_comp_filter_omega, &(_parameters.height_comp_filter_omega));
-	param_get(_parameter_handles.speed_comp_filter_omega, &(_parameters.speed_comp_filter_omega));
-	param_get(_parameter_handles.roll_throttle_compensation, &(_parameters.roll_throttle_compensation));
-	param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
-	param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
-	param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
-
-	param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
-	param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
-
 	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
 	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
 	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
@@ -609,17 +537,7 @@ FixedwingPositionControl::vehicle_control_mode_poll()
 	orb_check(_control_mode_sub, &vstatus_updated);
 
 	if (vstatus_updated) {
-
-		bool was_armed = _control_mode.flag_armed;
-
 		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
-
-		if (!was_armed && _control_mode.flag_armed) {
-			_launch_lat = _global_pos.lat;
-			_launch_lon = _global_pos.lon;
-			_launch_alt = _global_pos.alt;
-			_launch_valid = true;
-		}
 	}
 }
 
@@ -815,9 +733,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
-	/* filter speed and altitude for controller */
-	math::Vector<3> accel_body(_sensor_combined.accelerometer_m_s2);
-
+	/* define altitude error */
 	float altitude_error = _pos_sp_triplet.current.alt - _global_pos.alt;
 
 	/* no throttle limit as default */
@@ -949,7 +865,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
-			/* Calculate altitude of last ordinary waypoint L */
+			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
 			float L_altitude_rel = _pos_sp_triplet.previous.valid ? _pos_sp_triplet.previous.alt - _pos_sp_triplet.current.alt : 0.0f;
 
 			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
@@ -1115,15 +1031,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			}
 		}
 
-		// warnx("nav bearing: %8.4f bearing err: %8.4f target bearing: %8.4f", (double)_l1_control.nav_bearing(),
-		//       (double)_l1_control.bearing_error(), (double)_l1_control.target_bearing());
-		// warnx("prev wp: %8.4f/%8.4f, next wp: %8.4f/%8.4f prev:%s", (double)prev_wp(0), (double)prev_wp(1),
-		//       (double)next_wp(0), (double)next_wp(1), (pos_sp_triplet.previous_valid) ? "valid" : "invalid");
-
-		// XXX at this point we always want no loiter hold if a
-		// mission is active
-		_loiter_hold = false;
-
 		/* reset landing state */
 		if (pos_sp_triplet.current.type != SETPOINT_TYPE_LAND) {
 			reset_landing_state();
@@ -1139,89 +1046,6 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_reset_integral = true;
 		}
 
-	} else if (0/* posctrl mode enabled */) {
-
-		_was_pos_control_mode = false;
-
-		/** POSCTRL FLIGHT **/
-
-	if (0/* switched from another mode to posctrl */) {
-		_altctrl_hold_heading = _att.yaw;
-	}
-
-	if (0/* posctrl on and manual control yaw non-zero */) {
-		_altctrl_hold_heading = _att.yaw + _manual.r;
-	}
-
-	//XXX not used
-
-	/* climb out control */
-//		bool climb_out = false;
-//
-//		/* user wants to climb out */
-//		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
-//			climb_out = true;
-//		}
-
-	/* if in altctrl mode, set airspeed based on manual control */
-
-	// XXX check if ground speed undershoot should be applied here
-	float altctrl_airspeed = _parameters.airspeed_min +
-					(_parameters.airspeed_max - _parameters.airspeed_min) *
-					_manual.z;
-
-		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
-		_att_sp.roll_body = _l1_control.nav_roll();
-		_att_sp.yaw_body = _l1_control.nav_bearing();
-
-		tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
-					math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
-					_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-					false, math::radians(_parameters.pitch_limit_min), _global_pos.alt, ground_speed);
-
-	} else if (0/* altctrl mode enabled */) {
-
-		_was_pos_control_mode = false;
-
-		/** ALTCTRL FLIGHT **/
-
-		if (0/* switched from another mode to altctrl */) {
-			_altctrl_hold_heading = _att.yaw;
-		}
-
-		if (0/* altctrl on and manual control yaw non-zero */) {
-			_altctrl_hold_heading = _att.yaw + _manual.r;
-		}
-
-		/* if in altctrl mode, set airspeed based on manual control */
-
-		// XXX check if ground speed undershoot should be applied here
-		float altctrl_airspeed = _parameters.airspeed_min +
-					  (_parameters.airspeed_max - _parameters.airspeed_min) *
-					  _manual.z;
-
-		/* user switched off throttle */
-		if (_manual.z < 0.1f) {
-			throttle_max = 0.0f;
-		}
-
-		/* climb out control */
-		bool climb_out = false;
-
-		/* user wants to climb out */
-		if (_manual.x > 0.3f && _manual.z > 0.8f) {
-			climb_out = true;
-		}
-
-		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed_2d);
-		_att_sp.roll_body =	_manual.y;
-		_att_sp.yaw_body =	_manual.r;
-		tecs_update_pitch_throttle(_global_pos.alt + _manual.x * 2.0f, altctrl_airspeed, eas2tas,
-					math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max),
-					_parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-					climb_out, math::radians(_parameters.pitch_limit_min),
-					_global_pos.alt, ground_speed);
-
 	} else {
 
 		_was_pos_control_mode = false;
@@ -1339,14 +1163,6 @@ FixedwingPositionControl::task_main()
 				_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 			}
 
-			static uint64_t last_run = 0;
-			float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f;
-			last_run = hrt_absolute_time();
-
-			/* guard against too large deltaT's */
-			if (deltaT > 1.0f)
-				deltaT = 0.01f;
-
 			/* load local copies */
 			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_global_pos);
 
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 52128e1b7..27039ff51 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -155,182 +155,6 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
 PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
 
 /**
- * Maximum climb rate
- *
- * This is the best climb rate that the aircraft can achieve with 
- * the throttle set to THR_MAX and the airspeed set to the 
- * default value. For electric aircraft make sure this number can be 
- * achieved towards the end of flight when the battery voltage has reduced. 
- * The setting of this parameter can be checked by commanding a positive 
- * altitude change of 100m in loiter, RTL or guided mode. If the throttle 
- * required to climb is close to THR_MAX and the aircraft is maintaining 
- * airspeed, then this parameter is set correctly. If the airspeed starts 
- * to reduce, then the parameter is set to high, and if the throttle 
- * demand required to climb and maintain speed is noticeably less than 
- * FW_THR_MAX, then either FW_T_CLMB_MAX should be increased or 
- * FW_THR_MAX reduced.
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
-
-/**
- * Minimum descent rate
- *
- * This is the sink rate of the aircraft with the throttle 
- * set to THR_MIN and flown at the same airspeed as used 
- * to measure FW_T_CLMB_MAX.
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
-
-/**
- * Maximum descent rate
- *
- * This sets the maximum descent rate that the controller will use. 
- * If this value is too large, the aircraft can over-speed on descent. 
- * This should be set to a value that can be achieved without 
- * exceeding the lower pitch angle limit and without over-speeding 
- * the aircraft.
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
-
-/**
- * TECS time constant
- *
- * This is the time constant of the TECS control algorithm (in seconds). 
- * Smaller values make it faster to respond, larger values make it slower
- * to respond.
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
-
-/**
- * Throttle damping factor
- *
- * This is the damping gain for the throttle demand loop. 
- * Increase to add damping to correct for oscillations in speed and height.
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
-
-/**
- * Integrator gain
- *
- * This is the integrator gain on the control loop. 
- * Increasing this gain increases the speed at which speed 
- * and height offsets are trimmed out, but reduces damping and 
- * increases overshoot.
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
-
-/**
- * Maximum vertical acceleration
- *
- * This is the maximum vertical acceleration (in metres/second square)
- * either up or down that the controller will use to correct speed 
- * or height errors. The default value of 7 m/s/s (equivalent to +- 0.7 g) 
- * allows for reasonably aggressive pitch changes if required to recover 
- * from under-speed conditions.
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
-
-/**
- * Complementary filter "omega" parameter for height
- *
- * This is the cross-over frequency (in radians/second) of the complementary 
- * filter used to fuse vertical acceleration and barometric height to obtain 
- * an estimate of height rate and height. Increasing this frequency weights 
- * the solution more towards use of the barometer, whilst reducing it weights 
- * the solution more towards use of the accelerometer data.
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
-
-/**
- * Complementary filter "omega" parameter for speed
- *
- * This is the cross-over frequency (in radians/second) of the complementary 
- * filter used to fuse longitudinal acceleration and airspeed to obtain an 
- * improved airspeed estimate. Increasing this frequency weights the solution
- * more towards use of the arispeed sensor, whilst reducing it weights the 
- * solution more towards use of the accelerometer data.
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
-
-/**
- * Roll -> Throttle feedforward
- *
- * Increasing this gain turn increases the amount of throttle that will 
- * be used to compensate for the additional drag created by turning. 
- * Ideally this should be set to  approximately 10 x the extra sink rate 
- * in m/s created by a 45 degree bank turn. Increase this gain if 
- * the aircraft initially loses energy in turns and reduce if the 
- * aircraft initially gains energy in turns. Efficient high aspect-ratio 
- * aircraft (eg powered sailplanes) can use a lower value, whereas 
- * inefficient low aspect-ratio models (eg delta wings) can use a higher value.
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
-
-/**
- * Speed <--> Altitude priority
- *
- * This parameter adjusts the amount of weighting that the pitch control 
- * applies to speed vs height errors. Setting it to 0.0 will cause the 
- * pitch control to control height and ignore speed errors. This will 
- * normally improve height accuracy but give larger airspeed errors. 
- * Setting it to 2.0 will cause the pitch control loop to control speed 
- * and ignore height errors. This will normally reduce airspeed errors, 
- * but give larger height errors. The default value of 1.0 allows the pitch 
- * control to simultaneously control height and speed. 
- * Note to Glider Pilots - set this parameter to 2.0 (The glider will 
- * adjust its pitch angle to maintain airspeed, ignoring changes in height).
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
-
-/**
- * Pitch damping factor
- *
- * This is the damping gain for the pitch demand loop. Increase to add 
- * damping to correct for oscillations in height. The default value of 0.0 
- * will work well provided the pitch to servo controller has been tuned 
- * properly.
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
-
-/**
- * Height rate P factor
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
-
-/**
- * Speed rate P factor
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
-
-/**
  * Landing slope angle
  *
  * @group L1 Control
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 2e37d166e..749f57a2b 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -59,6 +59,7 @@ mTecs::mTecs() :
 	_controlAltitude(this, "FPA", true),
 	_controlAirSpeed(this, "ACC"),
 	_flightPathAngleLowpass(this, "FPA_LP"),
+	_altitudeLowpass(this, "ALT_LP"),
 	_airspeedLowpass(this, "A_LP"),
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
@@ -93,18 +94,23 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 	/* time measurement */
 	updateTimeMeasurement();
 
+	/* Filter altitude */
+	float altitudeFiltered = _altitudeLowpass.update(altitude);
+
+
 	/* calculate flight path angle setpoint from altitude setpoint */
-	float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitude);
+	float flightPathAngleSp = _controlAltitude.update(altitudeSp - altitudeFiltered);
 
 	/* Debug output */
 	if (_counter % 10 == 0) {
 		debug("***");
-		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)flightPathAngleSp);
+		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
 	}
 
 	/* Write part of the status message */
 	_status.altitudeSp = altitudeSp;
 	_status.altitude = altitude;
+	_status.altitudeFiltered = altitudeFiltered;
 
 
 	/* use flightpath angle setpoint for total energy control */
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.h b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
index efa89a5d3..ae6867d38 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.h
@@ -115,6 +115,7 @@ protected:
 
 	/* Other calculation Blocks */
 	control::BlockLowPass _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
+	control::BlockLowPass _altitudeLowpass;	/**< low pass filter for altitude */
 	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
 	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
 
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
index c22e60ae0..bb2065472 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_blocks.h
@@ -72,8 +72,8 @@ public:
 	 * @return: true if the limit is applied, false otherwise
 	 */
 	bool limit(float& value, float& difference) {
-		float minimum = isAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
-		float maximum = isAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
+		float minimum = getIsAngularLimit() ? getMin() * M_DEG_TO_RAD_F : getMin();
+		float maximum = getIsAngularLimit() ? getMax() * M_DEG_TO_RAD_F : getMax();
 		if (value < minimum) {
 			difference = value - minimum;
 			value = minimum;
@@ -86,7 +86,7 @@ public:
 		return false;
 	}
 //accessor:
-	bool isAngularLimit() {return _isAngularLimit ;}
+	bool getIsAngularLimit() {return _isAngularLimit ;}
 	float getMin() { return _min.get(); }
 	float getMax() { return _max.get(); }
 	void setMin(float value) { _min.set(value); }
diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
index 5b9238780..4ca31fe20 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs_params.c
@@ -175,6 +175,13 @@ PARAM_DEFINE_FLOAT(MT_PIT_MIN, -45.0f);
 PARAM_DEFINE_FLOAT(MT_PIT_MAX, 20.0f);
 
 /**
+ * Lowpass (cutoff freq.) for altitude
+ *
+ * @group mTECS
+ */
+PARAM_DEFINE_FLOAT(MT_ALT_LP, 1.0f);
+
+/**
  * Lowpass (cutoff freq.) for the flight path angle
  *
  * @group mTECS
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 75799804c..3f64f30ed 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -81,7 +81,7 @@
 #include "mavlink_commands.h"
 
 #ifndef MAVLINK_CRC_EXTRA
- #error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
+#error MAVLINK_CRC_EXTRA has to be defined on PX4 systems
 #endif
 
 /* oddly, ERROR is not defined for c++ */
@@ -154,7 +154,8 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 		instance = Mavlink::get_instance(6);
 		break;
 #endif
-		default:
+
+	default:
 		return;
 	}
 
@@ -169,17 +170,16 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 	int buf_free = 0;
 
 	if (instance->get_flow_control_enabled()
-		&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
+	    && ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
 
 		/* Disable hardware flow control:
 		 * if no successful write since a defined time
 		 * and if the last try was not the last successful write
 		 */
 		if (last_write_try_times[(unsigned)channel] != 0 &&
-			hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
-			last_write_success_times[(unsigned)channel] !=
-			last_write_try_times[(unsigned)channel])
-		{
+		    hrt_elapsed_time(&last_write_success_times[(unsigned)channel]) > 500 * 1000UL &&
+		    last_write_success_times[(unsigned)channel] !=
+		    last_write_try_times[(unsigned)channel]) {
 			warnx("DISABLING HARDWARE FLOW CONTROL");
 			instance->enable_flow_control(false);
 		}
@@ -197,15 +197,20 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 			if (buf_free < desired) {
 				/* we don't want to send anything just in half, so return */
 				instance->count_txerr();
+				instance->count_txerrbytes(desired);
 				return;
 			}
 		}
 
 		ssize_t ret = write(uart, ch, desired);
+
 		if (ret != desired) {
 			instance->count_txerr();
+			instance->count_txerrbytes(desired);
+
 		} else {
 			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
+			instance->count_txbytes(desired);
 		}
 	}
 }
@@ -233,9 +238,9 @@ Mavlink::Mavlink() :
 	_mission_result_sub(-1),
 	_mode(MAVLINK_MODE_NORMAL),
 	_channel(MAVLINK_COMM_0),
-	_logbuffer{},
+	_logbuffer {},
 	_total_counter(0),
-	_receive_thread{},
+	_receive_thread {},
 	_verbose(false),
 	_forwarding_on(false),
 	_passing_on(false),
@@ -248,8 +253,16 @@ Mavlink::Mavlink() :
 	_subscribe_to_stream(nullptr),
 	_subscribe_to_stream_rate(0.0f),
 	_flow_control_enabled(true),
-	_message_buffer{},
-	_message_buffer_mutex{},
+	_bytes_tx(0),
+	_bytes_txerr(0),
+	_bytes_rx(0),
+	_bytes_timestamp(0),
+	_rate_tx(0.0f),
+	_rate_txerr(0.0f),
+	_rate_rx(0.0f),
+	_rstatus {},
+	_message_buffer {},
+	_message_buffer_mutex {},
 	_param_initialized(false),
 	_param_system_id(0),
 	_param_component_id(0),
@@ -257,7 +270,7 @@ Mavlink::Mavlink() :
 	_param_use_hil_gps(0),
 	_param_forward_externalsp(0),
 
-/* performance counters */
+	/* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink_el")),
 	_txerr_perf(perf_alloc(PC_COUNT, "mavlink_txe"))
 {
@@ -426,6 +439,27 @@ Mavlink::destroy_all_instances()
 	return OK;
 }
 
+int
+Mavlink::get_status_all_instances()
+{
+	Mavlink *inst = ::_mavlink_instances;
+
+	unsigned iterations = 0;
+
+	while (inst != nullptr) {
+
+		printf("\ninstance #%u:\n", iterations);
+		inst->display_status();
+
+		/* move on */
+		inst = inst->next;
+		iterations++;
+	}
+
+	/* return an error if there are no instances */
+	return (iterations == 0);
+}
+
 bool
 Mavlink::instance_exists(const char *device_name, Mavlink *self)
 {
@@ -623,7 +657,8 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 	case 921600: speed = B921600; break;
 
 	default:
-		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n", baud);
+		warnx("ERROR: Unsupported baudrate: %d\n\tsupported examples:\n\t9600, 19200, 38400, 57600\t\n115200\n230400\n460800\n921600\n",
+		      baud);
 		return -EINVAL;
 	}
 
@@ -853,8 +888,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
 			mavlink_param_request_list_t req;
 			mavlink_msg_param_request_list_decode(msg, &req);
+
 			if (req.target_system == mavlink_system.sysid &&
-					(req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
+			    (req.target_component == mavlink_system.compid || req.target_component == MAV_COMP_ID_ALL)) {
 				/* Start sending parameters */
 				mavlink_pm_start_queued_send();
 				send_statustext_info("[pm] sending list");
@@ -869,7 +905,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 				mavlink_param_set_t mavlink_param_set;
 				mavlink_msg_param_set_decode(msg, &mavlink_param_set);
 
-				if (mavlink_param_set.target_system == mavlink_system.sysid && ((mavlink_param_set.target_component == mavlink_system.compid) || (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
+				if (mavlink_param_set.target_system == mavlink_system.sysid
+				    && ((mavlink_param_set.target_component == mavlink_system.compid)
+					|| (mavlink_param_set.target_component == MAV_COMP_ID_ALL))) {
 					/* local name buffer to enforce null-terminated string */
 					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
 					strncpy(name, mavlink_param_set.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
@@ -896,7 +934,9 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 			mavlink_param_request_read_t mavlink_param_request_read;
 			mavlink_msg_param_request_read_decode(msg, &mavlink_param_request_read);
 
-			if (mavlink_param_request_read.target_system == mavlink_system.sysid && ((mavlink_param_request_read.target_component == mavlink_system.compid) || (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
+			if (mavlink_param_request_read.target_system == mavlink_system.sysid
+			    && ((mavlink_param_request_read.target_component == mavlink_system.compid)
+				|| (mavlink_param_request_read.target_component == MAV_COMP_ID_ALL))) {
 				/* when no index is given, loop through string ids and compare them */
 				if (mavlink_param_request_read.param_index == -1) {
 					/* local name buffer to enforce null-terminated string */
@@ -959,15 +999,17 @@ Mavlink::send_statustext(unsigned severity, const char *string)
 
 		/* Map severity */
 		switch (severity) {
-			case MAVLINK_IOC_SEND_TEXT_INFO:
-				statustext.severity = MAV_SEVERITY_INFO;
-				break;
-			case MAVLINK_IOC_SEND_TEXT_CRITICAL:
-				statustext.severity = MAV_SEVERITY_CRITICAL;
-				break;
-			case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
-				statustext.severity = MAV_SEVERITY_EMERGENCY;
-				break;
+		case MAVLINK_IOC_SEND_TEXT_INFO:
+			statustext.severity = MAV_SEVERITY_INFO;
+			break;
+
+		case MAVLINK_IOC_SEND_TEXT_CRITICAL:
+			statustext.severity = MAV_SEVERITY_CRITICAL;
+			break;
+
+		case MAVLINK_IOC_SEND_TEXT_EMERGENCY:
+			statustext.severity = MAV_SEVERITY_EMERGENCY;
+			break;
 		}
 
 		mavlink_msg_statustext_send(_channel, statustext.severity, statustext.text);
@@ -1084,12 +1126,14 @@ Mavlink::message_buffer_init(int size)
 	_message_buffer.size = size;
 	_message_buffer.write_ptr = 0;
 	_message_buffer.read_ptr = 0;
-	_message_buffer.data = (char*)malloc(_message_buffer.size);
+	_message_buffer.data = (char *)malloc(_message_buffer.size);
 
 	int ret;
+
 	if (_message_buffer.data == 0) {
 		ret = ERROR;
 		_message_buffer.size = 0;
+
 	} else {
 		ret = OK;
 	}
@@ -1403,7 +1447,7 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("GPS_GLOBAL_ORIGIN", 0.5f);
 		configure_stream("HIGHRES_IMU", 1.0f * rate_mult);
 		configure_stream("ATTITUDE", 10.0f * rate_mult);
-		configure_stream("VFR_HUD", 10.0f * rate_mult);
+		configure_stream("VFR_HUD", 8.0f * rate_mult);
 		configure_stream("GPS_RAW_INT", 1.0f * rate_mult);
 		configure_stream("GLOBAL_POSITION_INT", 3.0f * rate_mult);
 		configure_stream("LOCAL_POSITION_NED", 3.0f * rate_mult);
@@ -1461,7 +1505,8 @@ Mavlink::task_main(int argc, char *argv[])
 		if (_subscribe_to_stream != nullptr) {
 			if (OK == configure_stream(_subscribe_to_stream, _subscribe_to_stream_rate)) {
 				if (_subscribe_to_stream_rate > 0.0f) {
-					warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name, (double)_subscribe_to_stream_rate);
+					warnx("stream %s on device %s enabled with rate %.1f Hz", _subscribe_to_stream, _device_name,
+					      (double)_subscribe_to_stream_rate);
 
 				} else {
 					warnx("stream %s on device %s disabled", _subscribe_to_stream, _device_name);
@@ -1500,48 +1545,63 @@ Mavlink::task_main(int argc, char *argv[])
 
 			bool is_part;
 			uint8_t *read_ptr;
-            uint8_t *write_ptr;
+			uint8_t *write_ptr;
 
 			pthread_mutex_lock(&_message_buffer_mutex);
-			int available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
+			int available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
 			pthread_mutex_unlock(&_message_buffer_mutex);
 
 			if (available > 0) {
-                // Reconstruct message from buffer
+				// Reconstruct message from buffer
 
 				mavlink_message_t msg;
-                write_ptr = (uint8_t*)&msg;
+				write_ptr = (uint8_t *)&msg;
 
-                // Pull a single message from the buffer
-                size_t read_count = available;
-                if (read_count > sizeof(mavlink_message_t)) {
-                    read_count = sizeof(mavlink_message_t);
-                }
+				// Pull a single message from the buffer
+				size_t read_count = available;
 
-                memcpy(write_ptr, read_ptr, read_count);
+				if (read_count > sizeof(mavlink_message_t)) {
+					read_count = sizeof(mavlink_message_t);
+				}
+
+				memcpy(write_ptr, read_ptr, read_count);
 
-                // We hold the mutex until after we complete the second part of the buffer. If we don't
-                // we may end up breaking the empty slot overflow detection semantics when we mark the
-                // possibly partial read below.
-                pthread_mutex_lock(&_message_buffer_mutex);
+				// We hold the mutex until after we complete the second part of the buffer. If we don't
+				// we may end up breaking the empty slot overflow detection semantics when we mark the
+				// possibly partial read below.
+				pthread_mutex_lock(&_message_buffer_mutex);
 
 				message_buffer_mark_read(read_count);
 
 				/* write second part of buffer if there is some */
 				if (is_part && read_count < sizeof(mavlink_message_t)) {
-                    write_ptr += read_count;
-					available = message_buffer_get_ptr((void**)&read_ptr, &is_part);
-                    read_count = sizeof(mavlink_message_t) - read_count;
-                    memcpy(write_ptr, read_ptr, read_count);
+					write_ptr += read_count;
+					available = message_buffer_get_ptr((void **)&read_ptr, &is_part);
+					read_count = sizeof(mavlink_message_t) - read_count;
+					memcpy(write_ptr, read_ptr, read_count);
 					message_buffer_mark_read(available);
 				}
 
-                pthread_mutex_unlock(&_message_buffer_mutex);
+				pthread_mutex_unlock(&_message_buffer_mutex);
 
-                _mavlink_resend_uart(_channel, &msg);
+				_mavlink_resend_uart(_channel, &msg);
 			}
 		}
 
+		/* update TX/RX rates*/
+		if (t > _bytes_timestamp + 1000000) {
+			if (_bytes_timestamp != 0) {
+				float dt = (t - _bytes_timestamp) / 1000.0f;
+				_rate_tx = _bytes_tx / dt;
+				_rate_txerr = _bytes_txerr / dt;
+				_rate_rx = _bytes_rx / dt;
+				_bytes_tx = 0;
+				_bytes_txerr = 0;
+				_bytes_rx = 0;
+			}
+			_bytes_timestamp = t;
+		}
+
 		perf_end(_loop_perf);
 	}
 
@@ -1592,6 +1652,7 @@ Mavlink::task_main(int argc, char *argv[])
 		message_buffer_destroy();
 		pthread_mutex_destroy(&_message_buffer_mutex);
 	}
+
 	/* destroy log buffer */
 	mavlink_logbuffer_destroy(&_logbuffer);
 
@@ -1613,6 +1674,7 @@ int Mavlink::start_helper(int argc, char *argv[])
 		/* out of memory */
 		res = -ENOMEM;
 		warnx("OUT OF MEM");
+
 	} else {
 		/* this will actually only return once MAVLink exits */
 		res = instance->task_main(argc, argv);
@@ -1672,7 +1734,40 @@ Mavlink::start(int argc, char *argv[])
 void
 Mavlink::display_status()
 {
-	warnx("running");
+
+	if (_rstatus.heartbeat_time > 0) {
+		printf("\tGCS heartbeat:\t%llu us ago\n", hrt_elapsed_time(&_rstatus.heartbeat_time));
+	}
+
+	if (_rstatus.timestamp > 0) {
+
+		printf("\ttype:\t\t");
+
+		switch (_rstatus.type) {
+		case TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO:
+			printf("3DR RADIO\n");
+			break;
+
+		default:
+			printf("UNKNOWN RADIO\n");
+			break;
+		}
+
+		printf("\trssi:\t\t%d\n", _rstatus.rssi);
+		printf("\tremote rssi:\t%u\n", _rstatus.remote_rssi);
+		printf("\ttxbuf:\t\t%u\n", _rstatus.txbuf);
+		printf("\tnoise:\t\t%d\n", _rstatus.noise);
+		printf("\tremote noise:\t%u\n", _rstatus.remote_noise);
+		printf("\trx errors:\t%u\n", _rstatus.rxerrors);
+		printf("\tfixed:\t\t%u\n", _rstatus.fixed);
+
+	} else {
+		printf("\tno telem status.\n");
+	}
+	printf("\trates:\n");
+	printf("\ttx: %.3f kB/s\n", (double)_rate_tx);
+	printf("\ttxerr: %.3f kB/s\n", (double)_rate_txerr);
+	printf("\trx: %.3f kB/s\n", (double)_rate_rx);
 }
 
 int
@@ -1760,8 +1855,8 @@ int mavlink_main(int argc, char *argv[])
 	} else if (!strcmp(argv[1], "stop-all")) {
 		return Mavlink::destroy_all_instances();
 
-		// } else if (!strcmp(argv[1], "status")) {
-		// 	mavlink::g_mavlink->status();
+	} else if (!strcmp(argv[1], "status")) {
+		return Mavlink::get_status_all_instances();
 
 	} else if (!strcmp(argv[1], "stream")) {
 		return Mavlink::stream_command(argc, argv);
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index f3882270c..260a7a224 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -51,6 +51,7 @@
 #include <uORB/uORB.h>
 #include <uORB/topics/mission.h>
 #include <uORB/topics/mission_result.h>
+#include <uORB/topics/telemetry_status.h>
 
 #include "mavlink_bridge_header.h"
 #include "mavlink_orb_subscription.h"
@@ -97,6 +98,8 @@ public:
 
 	static int		destroy_all_instances();
 
+	static int		get_status_all_instances();
+
 	static bool		instance_exists(const char *device_name, Mavlink *self);
 
 	static void		forward_message(const mavlink_message_t *msg, Mavlink *self);
@@ -231,6 +234,26 @@ public:
 	 */
 	void			count_txerr();
 
+	/**
+	 * Count transmitted bytes
+	 */
+	void			count_txbytes(unsigned n) { _bytes_tx += n; };
+
+	/**
+	 * Count bytes not transmitted because of errors
+	 */
+	void			count_txerrbytes(unsigned n) { _bytes_txerr += n; };
+
+	/**
+	 * Count received bytes
+	 */
+	void			count_rxbytes(unsigned n) { _bytes_rx += n; };
+
+	/**
+	 * Get the receive status of this MAVLink link
+	 */
+	struct telemetry_status_s&	get_rx_status() { return _rstatus; }
+
 protected:
 	Mavlink			*next;
 
@@ -253,13 +276,13 @@ private:
 	MavlinkOrbSubscription	*_subscriptions;
 	MavlinkStream		*_streams;
 
-	MavlinkMissionManager *_mission_manager;
+	MavlinkMissionManager	*_mission_manager;
 
-	orb_advert_t	_mission_pub;
+	orb_advert_t		_mission_pub;
 	int			_mission_result_sub;
-	MAVLINK_MODE _mode;
+	MAVLINK_MODE 		_mode;
 
-	mavlink_channel_t _channel;
+	mavlink_channel_t	_channel;
 
 	struct mavlink_logbuffer _logbuffer;
 	unsigned int		_total_counter;
@@ -288,6 +311,16 @@ private:
 
 	bool			_flow_control_enabled;
 
+	unsigned		_bytes_tx;
+	unsigned		_bytes_txerr;
+	unsigned		_bytes_rx;
+	uint64_t		_bytes_timestamp;
+	float		_rate_tx;
+	float		_rate_txerr;
+	float		_rate_rx;
+
+	struct telemetry_status_s	_rstatus;			///< receive status
+
 	struct mavlink_message_buffer {
 		int write_ptr;
 		int read_ptr;
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 7458e09f7..b39aebf9e 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -112,7 +112,6 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_telemetry_status_pub(-1),
 	_rc_pub(-1),
 	_manual_pub(-1),
-	_telemetry_heartbeat_time(0),
 	_radio_status_available(false),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
 	_hil_frames(0),
@@ -594,11 +593,11 @@ MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 		mavlink_radio_status_t rstatus;
 		mavlink_msg_radio_status_decode(msg, &rstatus);
 
-		struct telemetry_status_s tstatus;
-		memset(&tstatus, 0, sizeof(tstatus));
+		struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
 
 		tstatus.timestamp = hrt_absolute_time();
-		tstatus.heartbeat_time = _telemetry_heartbeat_time;
+		tstatus.telem_time = tstatus.timestamp;
+		/* tstatus.heartbeat_time is set by system heartbeats */
 		tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_3DR_RADIO;
 		tstatus.rssi = rstatus.rssi;
 		tstatus.remote_rssi = rstatus.remrssi;
@@ -655,16 +654,20 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg)
 
 		/* ignore own heartbeats, accept only heartbeats from GCS */
 		if (msg->sysid != mavlink_system.sysid && hb.type == MAV_TYPE_GCS) {
-			_telemetry_heartbeat_time = hrt_absolute_time();
+
+			struct telemetry_status_s &tstatus = _mavlink->get_rx_status();
+
+			hrt_abstime tnow = hrt_absolute_time();
+
+			/* always set heartbeat, publish only if telemetry link not up */
+			tstatus.heartbeat_time = tnow;
 
 			/* if no radio status messages arrive, lets at least publish that heartbeats were received */
 			if (!_radio_status_available) {
 
-				struct telemetry_status_s tstatus;
-				memset(&tstatus, 0, sizeof(tstatus));
-
-				tstatus.timestamp = _telemetry_heartbeat_time;
-				tstatus.heartbeat_time = _telemetry_heartbeat_time;
+				tstatus.timestamp = tnow;
+				/* telem_time indicates the timestamp of a telemetry status packet and we got none */
+				tstatus.telem_time = 0;
 				tstatus.type = TELEMETRY_STATUS_RADIO_TYPE_GENERIC;
 
 				if (_telemetry_status_pub < 0) {
@@ -1149,6 +1152,9 @@ MavlinkReceiver::receive_thread(void *arg)
 					_mavlink->handle_message(&msg);
 				}
 			}
+
+			/* count received bytes */
+			_mavlink->count_rxbytes(nread);
 		}
 	}
 
diff --git a/src/modules/mavlink/mavlink_receiver.h b/src/modules/mavlink/mavlink_receiver.h
index fc2b2a49b..a6553cb0a 100644
--- a/src/modules/mavlink/mavlink_receiver.h
+++ b/src/modules/mavlink/mavlink_receiver.h
@@ -152,7 +152,6 @@ private:
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
-	hrt_abstime _telemetry_heartbeat_time;
 	bool _radio_status_available;
 	int _control_mode_sub;
 	int _hil_frames;
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index dd7f4c801..606521f20 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -135,12 +135,15 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 			}
 		}
 
-		if (home_alt > missionitem.altitude) {
+		/* calculate the global waypoint altitude */
+		float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
+
+		if (home_alt > wp_alt) {
 			if (throw_error) {
-				mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
+				mavlink_log_critical(_mavlink_fd, "Rejecting Mission: Waypoint %d below home", i);
 				return false;
 			} else	{
-				mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
+				mavlink_log_critical(_mavlink_fd, "Warning: Waypoint %d below home", i);
 				return true;
 			}
 		}
diff --git a/src/modules/navigator/module.mk b/src/modules/navigator/module.mk
index 637eaae59..b50198996 100644
--- a/src/modules/navigator/module.mk
+++ b/src/modules/navigator/module.mk
@@ -54,3 +54,5 @@ SRCS		= navigator_main.cpp \
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
 
 MODULE_STACKSIZE = 1200
+
+EXTRACXXFLAGS	= -Weffc++
diff --git a/src/modules/navigator/navigator.h b/src/modules/navigator/navigator.h
index bf6e2ea0e..8edbb63b3 100644
--- a/src/modules/navigator/navigator.h
+++ b/src/modules/navigator/navigator.h
@@ -222,5 +222,11 @@ private:
 	 * Publish a new position setpoint triplet for position controllers
 	 */
 	void		publish_position_setpoint_triplet();
+
+	/* this class has ptr data members, so it should not be copied,
+	 * consequently the copy constructors are private.
+	 */
+	Navigator(const Navigator&);
+	Navigator operator=(const Navigator&);
 };
 #endif
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 1a5ba4c1a..331a9a728 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -105,17 +105,18 @@ Navigator::Navigator() :
 	_control_mode_sub(-1),
 	_onboard_mission_sub(-1),
 	_offboard_mission_sub(-1),
+	_param_update_sub(-1),
 	_pos_sp_triplet_pub(-1),
-	_vstatus({}),
-	_control_mode({}),
-	_global_pos({}),
-	_home_pos({}),
-	_mission_item({}),
-	_nav_caps({}),
-	_pos_sp_triplet({}),
+	_vstatus{},
+	_control_mode{},
+	_global_pos{},
+	_home_pos{},
+	_mission_item{},
+	_nav_caps{},
+	_pos_sp_triplet{},
 	_mission_item_valid(false),
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
-	_geofence({}),
+	_geofence{},
 	_geofence_violation_warning_sent(false),
 	_fence_valid(false),
 	_inside_fence(true),
@@ -124,6 +125,8 @@ Navigator::Navigator() :
 	_loiter(this, "LOI"),
 	_rtl(this, "RTL"),
 	_offboard(this, "OFF"),
+	_can_loiter_at_sp(false),
+	_pos_sp_triplet_updated(false),
 	_param_loiter_radius(this, "LOITER_RAD"),
 	_param_acceptance_radius(this, "ACC_RAD")
 {
@@ -455,7 +458,7 @@ void
 Navigator::publish_position_setpoint_triplet()
 {
 	/* update navigation state */
-	/* TODO: set nav_state */
+	_pos_sp_triplet.nav_state = _vstatus.nav_state;
 
 	/* lazily publish the position setpoint triplet only once available */
 	if (_pos_sp_triplet_pub > 0) {
diff --git a/src/modules/navigator/navigator_mode.h b/src/modules/navigator/navigator_mode.h
index a7ba79bba..de5545dcb 100644
--- a/src/modules/navigator/navigator_mode.h
+++ b/src/modules/navigator/navigator_mode.h
@@ -88,6 +88,12 @@ protected:
 
 private:
 	bool _first_run;
+
+	/* this class has ptr data members, so it should not be copied,
+	 * consequently the copy constructors are private.
+	 */
+	NavigatorMode(const NavigatorMode&);
+	NavigatorMode operator=(const NavigatorMode&);
 };
 
 #endif
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.c b/src/modules/position_estimator_inav/position_estimator_inav_params.c
index 0581f8236..98b31d17b 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -40,22 +40,196 @@
 
 #include "position_estimator_inav_params.h"
 
+/**
+ * Z axis weight for barometer
+ *
+ * Weight (cutoff frequency) for barometer altitude measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
+
+/**
+ * Z axis weight for GPS
+ *
+ * Weight (cutoff frequency) for GPS altitude measurements. GPS altitude data is very noisy and should be used only as slow correction for baro offset.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
+
+/**
+ * Z axis weight for sonar
+ *
+ * Weight (cutoff frequency) for sonar measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
+
+/**
+ * XY axis weight for GPS position
+ *
+ * Weight (cutoff frequency) for GPS position measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
+
+/**
+ * XY axis weight for GPS velocity
+ *
+ * Weight (cutoff frequency) for GPS velocity measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
+
+/**
+ * XY axis weight for optical flow
+ *
+ * Weight (cutoff frequency) for optical flow (velocity) measurements.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
+
+/**
+ * XY axis weight for resetting velocity
+ *
+ * When velocity sources lost slowly decrease estimated horizontal velocity with this weight.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_XY_RES_V, 0.5f);
+
+/**
+ * XY axis weight factor for GPS when optical flow available
+ *
+ * When optical flow data available, multiply GPS weights (for position and velocity) by this factor.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
+
+/**
+ * Accelerometer bias estimation weight
+ *
+ * Weight (cutoff frequency) for accelerometer bias estimation. 0 to disable.
+ *
+ * @min 0.0
+ * @max 0.1
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
+
+/**
+ * Optical flow scale factor
+ *
+ * Factor to convert raw optical flow (in pixels) to radians [rad/px].
+ *
+ * @min 0.0
+ * @max 1.0
+ * @unit rad/px
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_FLOW_K, 0.15f);
+
+/**
+ * Minimal acceptable optical flow quality
+ *
+ * 0 - lowest quality, 1 - best quality.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_FLOW_Q_MIN, 0.5f);
+
+/**
+ * Weight for sonar filter
+ *
+ * Sonar filter detects spikes on sonar measurements and used to detect new surface level.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_SONAR_FILT, 0.05f);
+
+/**
+ * Sonar maximal error for new surface
+ *
+ * If sonar measurement error is larger than this value it skiped (spike) or accepted as new surface level (if offset is stable).
+ *
+ * @min 0.0
+ * @max 1.0
+ * @unit m
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
+
+/**
+ * Land detector time
+ *
+ * Vehicle assumed landed if no altitude changes happened during this time on low throttle.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @unit s
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
+
+/**
+ * Land detector altitude dispersion threshold
+ *
+ * Dispersion threshold for triggering land detector.
+ *
+ * @min 0.0
+ * @max 10.0
+ * @unit m
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
+
+/**
+ * Land detector throttle threshold
+ *
+ * Value should be lower than minimal hovering thrust. Half of it is good choice.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
+
+/**
+ * GPS delay
+ *
+ * GPS delay compensation
+ *
+ * @min 0.0
+ * @max 1.0
+ * @unit s
+ * @group Position Estimator INAV
+ */
 PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index 2f721bf1e..606c639f9 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -111,7 +111,7 @@ mixer_tick(void)
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_FMU_OK;
 	}
 
-	/* default to failsafe mixing */
+	/* default to failsafe mixing - it will be forced below if flag is set */
 	source = MIX_FAILSAFE;
 
 	/*
@@ -155,6 +155,13 @@ mixer_tick(void)
 	}
 
 	/*
+	 * Check if we should force failsafe - and do it if we have to
+	 */
+	if (r_setup_arming & PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE) {
+		source = MIX_FAILSAFE;
+	}
+
+	/*
 	 * Set failsafe status flag depending on mixing source
 	 */
 	if (source == MIX_FAILSAFE) {
diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index d5a6b1ec4..050783687 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -179,6 +179,7 @@
 #define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE	(1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */
 #define PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED	(1 << 6) /* Disable the IO-internal evaluation of the RC */
 #define PX4IO_P_SETUP_ARMING_LOCKDOWN		(1 << 7) /* If set, the system operates normally, but won't actuate any servos */
+#define PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE	(1 << 8) /* If set, the system will always output the failsafe values */
 
 #define PX4IO_P_SETUP_PWM_RATES			2	/* bitmask, 0 = low rate, 1 = high rate */
 #define PX4IO_P_SETUP_PWM_DEFAULTRATE		3	/* 'low' PWM frame output rate in Hz */
@@ -253,6 +254,10 @@ enum {							/* DSM bind states */
 /* PWM failsafe values - zero disables the output */
 #define PX4IO_PAGE_FAILSAFE_PWM			55		/**< 0..CONFIG_ACTUATOR_COUNT-1 */
 
+/* PWM failsafe values - zero disables the output */
+#define PX4IO_PAGE_SENSORS			56		/**< Sensors connected to PX4IO */
+#define PX4IO_P_SENSORS_ALTITUDE		0		/**< Altitude of an external sensor (HoTT or S.BUS2) */
+
 /* Debug and test page - not used in normal operation */
 #define PX4IO_PAGE_TEST				127
 #define PX4IO_P_TEST_LED			0		/**< set the amber LED on/off */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index b37259997..43161aa70 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -189,7 +189,8 @@ volatile uint16_t	r_page_setup[] =
 					 PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \
 					 PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE | \
 					 PX4IO_P_SETUP_ARMING_RC_HANDLING_DISABLED | \
-					 PX4IO_P_SETUP_ARMING_LOCKDOWN)
+					 PX4IO_P_SETUP_ARMING_LOCKDOWN | \
+					 PX4IO_P_SETUP_ARMING_FORCE_FAILSAFE)
 #define PX4IO_P_SETUP_RATES_VALID	((1 << PX4IO_SERVO_COUNT) - 1)
 #define PX4IO_P_SETUP_RELAYS_VALID	((1 << PX4IO_RELAY_CHANNELS) - 1)
 
@@ -738,30 +739,19 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val
 		{
 			/*
 			 * Coefficients here derived by measurement of the 5-16V
-			 * range on one unit:
+			 * range on one unit, validated on sample points of another unit
 			 *
-			 * V   counts
-			 *  5  1001
-			 *  6  1219
-			 *  7  1436
-			 *  8  1653
-			 *  9  1870
-			 * 10  2086
-			 * 11  2303
-			 * 12  2522
-			 * 13  2738
-			 * 14  2956
-			 * 15  3172
-			 * 16  3389
+			 * Data in Tools/tests-host/data folder.
 			 *
-			 * slope = 0.0046067
-			 * intercept = 0.3863
+			 * measured slope = 0.004585267878277 (int: 4585)
+			 * nominal theoretic slope: 0.00459340659 (int: 4593)
+			 * intercept = 0.016646394188076 (int: 16646)
+			 * nominal theoretic intercept: 0.00 (int: 0)
 			 *
-			 * Intercept corrected for best results @ 12V.
 			 */
 			unsigned counts = adc_measure(ADC_VBATT);
 			if (counts != 0xffff) {
-				unsigned mV = (4150 + (counts * 46)) / 10 - 200;
+				unsigned mV = (166460 + (counts * 45934)) / 10000;
 				unsigned corrected = (mV * r_page_setup[PX4IO_P_SETUP_VBATT_SCALE]) / 10000;
 
 				r_page_status[PX4IO_P_STATUS_VBATT] = corrected;
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index f534c0f4c..31861c3fc 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1432,17 +1432,20 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 		/* --- GLOBAL POSITION SETPOINT --- */
 		if (copy_if_updated(ORB_ID(position_setpoint_triplet), subs.triplet_sub, &buf.triplet)) {
-			log_msg.msg_type = LOG_GPSP_MSG;
-			log_msg.body.log_GPSP.nav_state = 0;  /* TODO: Fix this */
-			log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
-			log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
-			log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
-			log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
-			log_msg.body.log_GPSP.type = buf.triplet.current.type;
-			log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
-			log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
-			log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
-			LOGBUFFER_WRITE_AND_COUNT(GPSP);
+
+			if (buf.triplet.current.valid) {
+				log_msg.msg_type = LOG_GPSP_MSG;
+				log_msg.body.log_GPSP.nav_state = buf.triplet.nav_state;
+				log_msg.body.log_GPSP.lat = (int32_t)(buf.triplet.current.lat * 1e7d);
+				log_msg.body.log_GPSP.lon = (int32_t)(buf.triplet.current.lon * 1e7d);
+				log_msg.body.log_GPSP.alt = buf.triplet.current.alt;
+				log_msg.body.log_GPSP.yaw = buf.triplet.current.yaw;
+				log_msg.body.log_GPSP.type = buf.triplet.current.type;
+				log_msg.body.log_GPSP.loiter_radius = buf.triplet.current.loiter_radius;
+				log_msg.body.log_GPSP.loiter_direction = buf.triplet.current.loiter_direction;
+				log_msg.body.log_GPSP.pitch_min = buf.triplet.current.pitch_min;
+				LOGBUFFER_WRITE_AND_COUNT(GPSP);
+			}
 		}
 
 		/* --- VICON POSITION --- */
@@ -1595,6 +1598,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.msg_type = LOG_TECS_MSG;
 			log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
 			log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
+			log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered;
 			log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
 			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
 			log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index b14ef04cc..853a3811f 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -334,6 +334,7 @@ struct log_GS1B_s {
 struct log_TECS_s {
 	float altitudeSp;
 	float altitude;
+	float altitudeFiltered;
 	float flightPathAngleSp;
 	float flightPathAngle;
 	float flightPathAngleFiltered;
@@ -454,7 +455,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "ffffffffffffffB",	"AltSP,Alt,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(TECS, "fffffffffffffffB",	"ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
index a98d3fc3a..0f6c9aca1 100644
--- a/src/modules/uORB/topics/actuator_armed.h
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -56,6 +56,7 @@ struct actuator_armed_s {
 	bool		armed;		/**< Set to true if system is armed */
 	bool		ready_to_arm;	/**< Set to true if system is ready to be armed */
 	bool		lockdown;	/**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */
+	bool		force_failsafe; /**< Set to true if the actuators are forced to the failsafe position */
 };
 
 /**
@@ -65,4 +66,4 @@ struct actuator_armed_s {
 /* register this as object request broker structure */
 ORB_DECLARE(actuator_armed);
 
-#endif
\ No newline at end of file
+#endif
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 673c0e491..4a1932180 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -95,6 +95,8 @@ struct position_setpoint_triplet_s
 	struct position_setpoint_s previous;
 	struct position_setpoint_s current;
 	struct position_setpoint_s next;
+
+	unsigned nav_state;				/**< report the navigation state */
 };
 
 /**
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
index c4d0c1874..33055018c 100644
--- a/src/modules/uORB/topics/tecs_status.h
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -66,6 +66,7 @@ struct tecs_status_s {
 
 	float altitudeSp;
 	float altitude;
+	float altitudeFiltered;
 	float flightPathAngleSp;
 	float flightPathAngle;
 	float flightPathAngleFiltered;
diff --git a/src/modules/uORB/topics/telemetry_status.h b/src/modules/uORB/topics/telemetry_status.h
index 3347724a5..1297c1a9d 100644
--- a/src/modules/uORB/topics/telemetry_status.h
+++ b/src/modules/uORB/topics/telemetry_status.h
@@ -57,7 +57,8 @@ enum TELEMETRY_STATUS_RADIO_TYPE {
 
 struct telemetry_status_s {
 	uint64_t timestamp;
-	uint64_t heartbeat_time;	/**< Time of last received heartbeat from remote system */
+	uint64_t heartbeat_time;		/**< Time of last received heartbeat from remote system */
+	uint64_t telem_time;			/**< Time of last received telemetry status packet, 0 for none */
 	enum TELEMETRY_STATUS_RADIO_TYPE type;	/**< type of the radio hardware     */
 	uint8_t rssi;				/**< local signal strength                      */
 	uint8_t remote_rssi;			/**< remote signal strength                     */
diff --git a/src/modules/uORB/topics/vehicle_command.h b/src/modules/uORB/topics/vehicle_command.h
index faf8cc7db..84503887d 100644
--- a/src/modules/uORB/topics/vehicle_command.h
+++ b/src/modules/uORB/topics/vehicle_command.h
@@ -79,6 +79,7 @@ enum VEHICLE_CMD {
 	VEHICLE_CMD_DO_REPEAT_RELAY = 182, /* Cycle a relay on and off for a desired number of cyles with a desired period. |Relay number| Cycle count| Cycle time (seconds, decimal)| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_SET_SERVO = 183, /* Set a servo to a desired PWM value. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_REPEAT_SERVO = 184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo number| PWM (microseconds, 1000 to 2000 typical)| Cycle count| Cycle time (seconds)| Empty| Empty| Empty|  */
+	VEHICLE_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_CONTROL_VIDEO = 200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds (decimal)| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty|  */
 	VEHICLE_CMD_DO_LAST = 240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty|  */
 	VEHICLE_CMD_PREFLIGHT_CALIBRATION = 241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. |Gyro calibration: 0: no, 1: yes| Magnetometer calibration: 0: no, 1: yes| Ground pressure: 0: no, 1: yes| Radio calibration: 0: no, 1: yes| Accelerometer calibration: 0: no, 1: yes| Empty| Empty|  */
diff --git a/src/modules/uavcan/.gitignore b/src/modules/uavcan/.gitignore
new file mode 100644
index 000000000..24fbf171f
--- /dev/null
+++ b/src/modules/uavcan/.gitignore
@@ -0,0 +1 @@
+./dsdlc_generated/
diff --git a/src/modules/uavcan/esc_controller.cpp b/src/modules/uavcan/esc_controller.cpp
new file mode 100644
index 000000000..406eba88c
--- /dev/null
+++ b/src/modules/uavcan/esc_controller.cpp
@@ -0,0 +1,138 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 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 esc_controller.cpp
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#include "esc_controller.hpp"
+#include <systemlib/err.h>
+
+UavcanEscController::UavcanEscController(uavcan::INode &node) :
+	_node(node),
+	_uavcan_pub_raw_cmd(node),
+	_uavcan_sub_status(node),
+	_orb_timer(node)
+{
+}
+
+UavcanEscController::~UavcanEscController()
+{
+	perf_free(_perfcnt_invalid_input);
+	perf_free(_perfcnt_scaling_error);
+}
+
+int UavcanEscController::init()
+{
+	// ESC status subscription
+	int res = _uavcan_sub_status.start(StatusCbBinder(this, &UavcanEscController::esc_status_sub_cb));
+	if (res < 0)
+	{
+		warnx("ESC status sub failed %i", res);
+		return res;
+	}
+
+	// ESC status will be relayed from UAVCAN bus into ORB at this rate
+	_orb_timer.setCallback(TimerCbBinder(this, &UavcanEscController::orb_pub_timer_cb));
+	_orb_timer.startPeriodic(uavcan::MonotonicDuration::fromMSec(1000 / ESC_STATUS_UPDATE_RATE_HZ));
+
+	return res;
+}
+
+void UavcanEscController::update_outputs(float *outputs, unsigned num_outputs)
+{
+	if ((outputs == nullptr) || (num_outputs > MAX_ESCS)) {
+		perf_count(_perfcnt_invalid_input);
+		return;
+	}
+
+	/*
+	 * Rate limiting - we don't want to congest the bus
+	 */
+	const auto timestamp = _node.getMonotonicTime();
+	if ((timestamp - _prev_cmd_pub).toUSec() < (1000000 / MAX_RATE_HZ)) {
+		return;
+	}
+	_prev_cmd_pub = timestamp;
+
+	/*
+	 * Fill the command message
+	 * If unarmed, we publish an empty message anyway
+	 */
+	uavcan::equipment::esc::RawCommand msg;
+
+	if (_armed) {
+		for (unsigned i = 0; i < num_outputs; i++) {
+
+			float scaled = (outputs[i] + 1.0F) * 0.5F * uavcan::equipment::esc::RawCommand::CMD_MAX;
+			if (scaled < 1.0F) {
+				scaled = 1.0F;  // Since we're armed, we don't want to stop it completely
+			}
+
+			if (scaled < uavcan::equipment::esc::RawCommand::CMD_MIN) {
+				scaled = uavcan::equipment::esc::RawCommand::CMD_MIN;
+				perf_count(_perfcnt_scaling_error);
+			} else if (scaled > uavcan::equipment::esc::RawCommand::CMD_MAX) {
+				scaled = uavcan::equipment::esc::RawCommand::CMD_MAX;
+				perf_count(_perfcnt_scaling_error);
+			} else {
+				; // Correct value
+			}
+
+			msg.cmd.push_back(static_cast<unsigned>(scaled));
+		}
+	}
+
+	/*
+	 * Publish the command message to the bus
+	 * Note that for a quadrotor it takes one CAN frame
+	 */
+	(void)_uavcan_pub_raw_cmd.broadcast(msg);
+}
+
+void UavcanEscController::arm_esc(bool arm)
+{
+	_armed = arm;
+}
+
+void UavcanEscController::esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg)
+{
+	// TODO save status into a local storage; publish to ORB later from orb_pub_timer_cb()
+}
+
+void UavcanEscController::orb_pub_timer_cb(const uavcan::TimerEvent&)
+{
+	// TODO publish to ORB
+}
diff --git a/src/modules/uavcan/esc_controller.hpp b/src/modules/uavcan/esc_controller.hpp
new file mode 100644
index 000000000..559ede561
--- /dev/null
+++ b/src/modules/uavcan/esc_controller.hpp
@@ -0,0 +1,107 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 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 esc_controller.hpp
+ *
+ * UAVCAN <--> ORB bridge for ESC messages:
+ *     uavcan.equipment.esc.RawCommand
+ *     uavcan.equipment.esc.RPMCommand
+ *     uavcan.equipment.esc.Status
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#pragma once
+
+#include <uavcan/uavcan.hpp>
+#include <uavcan/equipment/esc/RawCommand.hpp>
+#include <uavcan/equipment/esc/Status.hpp>
+#include <systemlib/perf_counter.h>
+
+class UavcanEscController
+{
+public:
+	UavcanEscController(uavcan::INode& node);
+	~UavcanEscController();
+
+	int init();
+
+	void update_outputs(float *outputs, unsigned num_outputs);
+
+	void arm_esc(bool arm);
+
+private:
+	/**
+	 * ESC status message reception will be reported via this callback.
+	 */
+	void esc_status_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status> &msg);
+
+	/**
+	 * ESC status will be published to ORB from this callback (fixed rate).
+	 */
+	void orb_pub_timer_cb(const uavcan::TimerEvent &event);
+
+
+	static constexpr unsigned MAX_RATE_HZ = 100;			///< XXX make this configurable
+	static constexpr unsigned ESC_STATUS_UPDATE_RATE_HZ = 5;
+	static constexpr unsigned MAX_ESCS = uavcan::equipment::esc::RawCommand::FieldTypes::cmd::MaxSize;
+
+	typedef uavcan::MethodBinder<UavcanEscController*,
+		void (UavcanEscController::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::esc::Status>&)>
+		StatusCbBinder;
+
+	typedef uavcan::MethodBinder<UavcanEscController*, void (UavcanEscController::*)(const uavcan::TimerEvent&)>
+		TimerCbBinder;
+
+	/*
+	 * libuavcan related things
+	 */
+	uavcan::MonotonicTime							_prev_cmd_pub;   ///< rate limiting
+	uavcan::INode								&_node;
+	uavcan::Publisher<uavcan::equipment::esc::RawCommand>			_uavcan_pub_raw_cmd;
+	uavcan::Subscriber<uavcan::equipment::esc::Status, StatusCbBinder>	_uavcan_sub_status;
+	uavcan::TimerEventForwarder<TimerCbBinder>				_orb_timer;
+
+	/*
+	 * ESC states
+	 */
+	bool 				_armed = false;
+	uavcan::equipment::esc::Status	_states[MAX_ESCS];
+
+	/*
+	 * Perf counters
+	 */
+	perf_counter_t _perfcnt_invalid_input = perf_alloc(PC_COUNT, "uavcan_esc_invalid_input");
+	perf_counter_t _perfcnt_scaling_error = perf_alloc(PC_COUNT, "uavcan_esc_scaling_error");
+};
diff --git a/src/modules/uavcan/gnss_receiver.cpp b/src/modules/uavcan/gnss_receiver.cpp
new file mode 100644
index 000000000..ba1fe5e49
--- /dev/null
+++ b/src/modules/uavcan/gnss_receiver.cpp
@@ -0,0 +1,153 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 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 gnss_receiver.cpp
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ * @author Andrew Chambers <achamber@gmail.com>
+ *
+ */
+
+#include "gnss_receiver.hpp"
+#include <systemlib/err.h>
+#include <mathlib/mathlib.h>
+
+#define MM_PER_CM 			10	// Millimeters per centimeter
+
+UavcanGnssReceiver::UavcanGnssReceiver(uavcan::INode &node) :
+_node(node),
+_uavcan_sub_status(node),
+_report_pub(-1)
+{
+}
+
+int UavcanGnssReceiver::init()
+{
+	int res = -1;
+
+	// GNSS fix subscription
+	res = _uavcan_sub_status.start(FixCbBinder(this, &UavcanGnssReceiver::gnss_fix_sub_cb));
+	if (res < 0)
+	{
+		warnx("GNSS fix sub failed %i", res);
+		return res;
+	}
+
+	// Clear the uORB GPS report
+	memset(&_report, 0, sizeof(_report));
+
+	return res;
+}
+
+void UavcanGnssReceiver::gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg)
+{
+	_report.timestamp_position = hrt_absolute_time();
+	_report.lat = msg.lat_1e7;
+	_report.lon = msg.lon_1e7;
+	_report.alt = msg.alt_1e2 * MM_PER_CM;	// Convert from centimeter (1e2) to millimeters (1e3)
+
+	_report.timestamp_variance = _report.timestamp_position;
+
+
+	// Check if the msg contains valid covariance information
+	const bool valid_position_covariance = !msg.position_covariance.empty();
+	const bool valid_velocity_covariance = !msg.velocity_covariance.empty();
+
+	if (valid_position_covariance) {
+		float pos_cov[9];
+		msg.position_covariance.unpackSquareMatrix(pos_cov);
+
+		// Horizontal position uncertainty
+		const float horizontal_pos_variance = math::max(pos_cov[0], pos_cov[4]);
+		_report.eph = (horizontal_pos_variance > 0) ? sqrtf(horizontal_pos_variance) : -1.0F;
+
+		// Vertical position uncertainty
+		_report.epv = (pos_cov[8] > 0) ? sqrtf(pos_cov[8]) : -1.0F;
+	} else {
+		_report.eph = -1.0F;
+		_report.epv = -1.0F;
+	}
+
+	if (valid_velocity_covariance) {
+	    float vel_cov[9];
+	    msg.velocity_covariance.unpackSquareMatrix(vel_cov);
+		_report.s_variance_m_s = math::max(math::max(vel_cov[0], vel_cov[4]), vel_cov[8]);
+
+		/* There is a nonlinear relationship between the velocity vector and the heading.
+		 * Use Jacobian to transform velocity covariance to heading covariance
+		 *
+		 * Nonlinear equation:
+		 * heading = atan2(vel_e_m_s, vel_n_m_s)
+		 * For math, see http://en.wikipedia.org/wiki/Atan2#Derivative
+		 *
+		 * To calculate the variance of heading from the variance of velocity,
+		 * cov(heading) = J(velocity)*cov(velocity)*J(velocity)^T
+		 */
+		float vel_n = msg.ned_velocity[0];
+		float vel_e = msg.ned_velocity[1];
+		float vel_n_sq = vel_n * vel_n;
+		float vel_e_sq = vel_e * vel_e;
+		_report.c_variance_rad =
+				(vel_e_sq * vel_cov[0] +
+						-2 * vel_n * vel_e * vel_cov[1] +	// Covariance matrix is symmetric
+						vel_n_sq* vel_cov[4]) / ((vel_n_sq + vel_e_sq) * (vel_n_sq + vel_e_sq));
+
+	} else {
+		_report.s_variance_m_s = -1.0F;
+		_report.c_variance_rad = -1.0F;
+	}
+
+	_report.fix_type = msg.status;
+
+	_report.timestamp_velocity = _report.timestamp_position;
+	_report.vel_n_m_s = msg.ned_velocity[0];
+	_report.vel_e_m_s = msg.ned_velocity[1];
+	_report.vel_d_m_s = msg.ned_velocity[2];
+	_report.vel_m_s = sqrtf(_report.vel_n_m_s * _report.vel_n_m_s + _report.vel_e_m_s * _report.vel_e_m_s + _report.vel_d_m_s * _report.vel_d_m_s);
+	_report.cog_rad = atan2f(_report.vel_e_m_s, _report.vel_n_m_s);
+	_report.vel_ned_valid = true;
+
+	_report.timestamp_time = _report.timestamp_position;
+	_report.time_gps_usec = uavcan::UtcTime(msg.gnss_timestamp).toUSec();	// Convert to microseconds
+
+	_report.satellites_used = msg.sats_used;
+
+	if (_report_pub > 0) {
+		orb_publish(ORB_ID(vehicle_gps_position), _report_pub, &_report);
+
+	} else {
+		_report_pub = orb_advertise(ORB_ID(vehicle_gps_position), &_report);
+	}
+
+}
diff --git a/src/modules/uavcan/gnss_receiver.hpp b/src/modules/uavcan/gnss_receiver.hpp
new file mode 100644
index 000000000..18df8da2f
--- /dev/null
+++ b/src/modules/uavcan/gnss_receiver.hpp
@@ -0,0 +1,84 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2014 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 gnss_receiver.hpp
+ *
+ * UAVCAN --> ORB bridge for GNSS messages:
+ *     uavcan.equipment.gnss.Fix
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ * @author Andrew Chambers <achamber@gmail.com>
+ */
+
+#pragma once
+
+#include <drivers/drv_hrt.h>
+
+#include <uORB/uORB.h>
+#include <uORB/topics/vehicle_gps_position.h>
+
+#include <uavcan/uavcan.hpp>
+#include <uavcan/equipment/gnss/Fix.hpp>
+
+class UavcanGnssReceiver
+{
+public:
+	UavcanGnssReceiver(uavcan::INode& node);
+
+	int init();
+
+private:
+	/**
+	 * GNSS fix message will be reported via this callback.
+	 */
+	void gnss_fix_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix> &msg);
+
+
+	typedef uavcan::MethodBinder<UavcanGnssReceiver*,
+		void (UavcanGnssReceiver::*)(const uavcan::ReceivedDataStructure<uavcan::equipment::gnss::Fix>&)>
+		FixCbBinder;
+
+	/*
+	 * libuavcan related things
+	 */
+	uavcan::INode													&_node;
+	uavcan::Subscriber<uavcan::equipment::gnss::Fix, FixCbBinder>	_uavcan_sub_status;
+
+	/*
+	 * uORB
+	 */
+	struct vehicle_gps_position_s 	_report;					///< uORB topic for gnss position
+	orb_advert_t			_report_pub;					///< uORB pub for gnss position
+
+};
diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk
new file mode 100644
index 000000000..1ef6f0cfa
--- /dev/null
+++ b/src/modules/uavcan/module.mk
@@ -0,0 +1,74 @@
+############################################################################
+#
+#   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+#   Author: Pavel Kirienko <pavel.kirienko@gmail.com>
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+############################################################################
+
+#
+# UAVCAN <--> uORB bridge
+#
+
+MODULE_COMMAND = uavcan
+
+MAXOPTIMIZATION = -Os
+
+SRCS += uavcan_main.cpp         \
+        uavcan_clock.cpp        \
+        esc_controller.cpp      \
+        gnss_receiver.cpp
+
+#
+# libuavcan
+#
+include $(UAVCAN_DIR)/libuavcan/include.mk
+SRCS += $(LIBUAVCAN_SRC)
+INCLUDE_DIRS += $(LIBUAVCAN_INC)
+# Since actual compiler mode is C++11, the library will default to UAVCAN_CPP11, but it will fail to compile
+# because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
+override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS
+
+#
+# libuavcan drivers for STM32
+#
+include $(UAVCAN_DIR)/libuavcan_drivers/stm32/driver/include.mk
+SRCS += $(LIBUAVCAN_STM32_SRC)
+INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
+override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2
+
+#
+# Invoke DSDL compiler
+# TODO: Add make target for this, or invoke dsdlc manually.
+#       The second option assumes that the generated headers shall be saved
+#       under the version control, which may be undesirable.
+#       The first option requires any Python and the Python Mako library for the sources to be built.
+#
+$(info $(shell $(LIBUAVCAN_DSDLC) $(UAVCAN_DSDL_DIR)))
+INCLUDE_DIRS += dsdlc_generated
diff --git a/src/modules/uavcan/uavcan_clock.cpp b/src/modules/uavcan/uavcan_clock.cpp
new file mode 100644
index 000000000..e41d5f953
--- /dev/null
+++ b/src/modules/uavcan/uavcan_clock.cpp
@@ -0,0 +1,79 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 <uavcan_stm32/uavcan_stm32.hpp>
+#include <drivers/drv_hrt.h>
+
+/**
+ * @file uavcan_clock.cpp
+ *
+ * Implements a clock for the CAN node.
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+namespace uavcan_stm32
+{
+namespace clock
+{
+
+uavcan::MonotonicTime getMonotonic()
+{
+	return uavcan::MonotonicTime::fromUSec(hrt_absolute_time());
+}
+
+uavcan::UtcTime getUtc()
+{
+	return uavcan::UtcTime();
+}
+
+void adjustUtc(uavcan::UtcDuration adjustment)
+{
+	(void)adjustment;
+}
+
+uavcan::uint64_t getUtcUSecFromCanInterrupt()
+{
+	return 0;
+}
+
+} // namespace clock
+
+SystemClock &SystemClock::instance()
+{
+	static SystemClock inst;
+	return inst;
+}
+
+}
+
diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
new file mode 100644
index 000000000..27e77e9c5
--- /dev/null
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -0,0 +1,582 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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 <nuttx/config.h>
+
+#include <cstdlib>
+#include <cstring>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <systemlib/mixer/mixer.h>
+#include <arch/board/board.h>
+#include <arch/chip/chip.h>
+
+#include <drivers/drv_hrt.h>
+#include <drivers/drv_pwm_output.h>
+
+#include "uavcan_main.hpp"
+
+/**
+ * @file uavcan_main.cpp
+ *
+ * Implements basic functinality of UAVCAN node.
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+/*
+ * UavcanNode
+ */
+UavcanNode *UavcanNode::_instance;
+
+UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
+	CDev("uavcan", UAVCAN_DEVICE_PATH),
+	_node(can_driver, system_clock),
+	_esc_controller(_node),
+	_gnss_receiver(_node)
+{
+	_control_topics[0] = ORB_ID(actuator_controls_0);
+	_control_topics[1] = ORB_ID(actuator_controls_1);
+	_control_topics[2] = ORB_ID(actuator_controls_2);
+	_control_topics[3] = ORB_ID(actuator_controls_3);
+
+	// memset(_controls, 0, sizeof(_controls));
+	// memset(_poll_fds, 0, sizeof(_poll_fds));
+}
+
+UavcanNode::~UavcanNode()
+{
+	if (_task != -1) {
+		/* tell the task we want it to go away */
+		_task_should_exit = true;
+
+		unsigned i = 10;
+
+		do {
+			/* wait 5ms - it should wake every 10ms or so worst-case */
+			usleep(5000);
+
+			/* if we have given up, kill it */
+			if (--i == 0) {
+				task_delete(_task);
+				break;
+			}
+
+		} while (_task != -1);
+	}
+
+	/* clean up the alternate device node */
+		// unregister_driver(PWM_OUTPUT_DEVICE_PATH);
+
+	::close(_armed_sub);
+
+	_instance = nullptr;
+}
+
+int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
+{
+	if (_instance != nullptr) {
+		warnx("Already started");
+		return -1;
+	}
+
+	/*
+	 * GPIO config.
+	 * Forced pull up on CAN2 is required for Pixhawk v1 where the second interface lacks a transceiver.
+	 * If no transceiver is connected, the RX pin will float, occasionally causing CAN controller to
+	 * fail during initialization.
+	 */
+	stm32_configgpio(GPIO_CAN1_RX);
+	stm32_configgpio(GPIO_CAN1_TX);
+	stm32_configgpio(GPIO_CAN2_RX | GPIO_PULLUP);
+	stm32_configgpio(GPIO_CAN2_TX);
+
+	/*
+	 * CAN driver init
+	 */
+	static CanInitHelper can;
+	static bool can_initialized = false;
+
+	if (!can_initialized) {
+		const int can_init_res = can.init(bitrate);
+
+		if (can_init_res < 0) {
+			warnx("CAN driver init failed %i", can_init_res);
+			return can_init_res;
+		}
+
+		can_initialized = true;
+	}
+
+	/*
+	 * Node init
+	 */
+	_instance = new UavcanNode(can.driver, uavcan_stm32::SystemClock::instance());
+
+	if (_instance == nullptr) {
+		warnx("Out of memory");
+		return -1;
+	}
+
+	const int node_init_res = _instance->init(node_id);
+
+	if (node_init_res < 0) {
+		delete _instance;
+		_instance = nullptr;
+		warnx("Node init failed %i", node_init_res);
+		return node_init_res;
+	}
+
+	/*
+	 * Start the task. Normally it should never exit.
+	 */
+	static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
+	_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_DEFAULT, StackSize,
+			      static_cast<main_t>(run_trampoline), nullptr);
+
+	if (_instance->_task < 0) {
+		warnx("start failed: %d", errno);
+		return -errno;
+	}
+
+	return OK;
+}
+
+int UavcanNode::init(uavcan::NodeID node_id)
+{
+	int ret = -1;
+
+	/* do regular cdev init */
+	ret = CDev::init();
+
+	if (ret != OK)
+		return ret;
+
+	ret = _esc_controller.init();
+	if (ret < 0)
+		return ret;
+
+	ret = _gnss_receiver.init();
+	if (ret < 0)
+		return ret;
+
+	uavcan::protocol::SoftwareVersion swver;
+	swver.major = 12;                        // TODO fill version info
+	swver.minor = 34;
+	_node.setSoftwareVersion(swver);
+
+	uavcan::protocol::HardwareVersion hwver;
+	hwver.major = 42;                        // TODO fill version info
+	hwver.minor = 42;
+	_node.setHardwareVersion(hwver);
+
+	_node.setName("org.pixhawk"); // Huh?
+
+	_node.setNodeID(node_id);
+
+	return _node.start();
+}
+
+void UavcanNode::node_spin_once()
+{
+	const int spin_res = _node.spin(uavcan::MonotonicTime());
+	if (spin_res < 0) {
+		warnx("node spin error %i", spin_res);
+	}
+}
+
+int UavcanNode::run()
+{
+	const unsigned PollTimeoutMs = 50;
+
+	// XXX figure out the output count
+	_output_count = 2;
+
+
+	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
+
+	actuator_outputs_s outputs;
+	memset(&outputs, 0, sizeof(outputs));
+
+	const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
+	if (busevent_fd < 0)
+	{
+		warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
+		_task_should_exit = true;
+	}
+
+	/*
+	 * XXX Mixing logic/subscriptions shall be moved into UavcanEscController::update();
+	 *     IO multiplexing shall be done here.
+	 */
+
+	_node.setStatusOk();
+
+	while (!_task_should_exit) {
+
+		if (_groups_subscribed != _groups_required) {
+			subscribe();
+			_groups_subscribed = _groups_required;
+			/*
+			 * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
+			 * Please note that with such multiplexing it is no longer possible to rely only on
+			 * the value returned from poll() to detect whether actuator control has timed out or not.
+			 * Instead, all ORB events need to be checked individually (see below).
+			 */
+			_poll_fds[_poll_fds_num] = ::pollfd();
+			_poll_fds[_poll_fds_num].fd = busevent_fd;
+			_poll_fds[_poll_fds_num].events = POLLIN;
+			_poll_fds_num += 1;
+		}
+
+		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
+
+		node_spin_once();  // Non-blocking
+
+		// this would be bad...
+		if (poll_ret < 0) {
+			log("poll error %d", errno);
+			continue;
+		} else {
+			// get controls for required topics
+			bool controls_updated = false;
+			unsigned poll_id = 0;
+			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+				if (_control_subs[i] > 0) {
+					if (_poll_fds[poll_id].revents & POLLIN) {
+						controls_updated = true;
+						orb_copy(_control_topics[i], _control_subs[i], &_controls[i]);
+					}
+					poll_id++;
+				}
+			}
+
+			if (!controls_updated) {
+				// timeout: no control data, switch to failsafe values
+				// XXX trigger failsafe
+			}
+
+			//can we mix?
+			if (controls_updated && (_mixers != nullptr)) {
+
+				// XXX one output group has 8 outputs max,
+				// but this driver could well serve multiple groups.
+				unsigned num_outputs_max = 8;
+
+				// Do mixing
+				outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs_max);
+				outputs.timestamp = hrt_absolute_time();
+
+				// iterate actuators
+				for (unsigned i = 0; i < outputs.noutputs; i++) {
+					// last resort: catch NaN, INF and out-of-band errors
+					if (!isfinite(outputs.output[i])) {
+						/*
+						 * Value is NaN, INF or out of band - set to the minimum value.
+						 * This will be clearly visible on the servo status and will limit the risk of accidentally
+						 * spinning motors. It would be deadly in flight.
+						 */
+						outputs.output[i] = -1.0f;
+					}
+
+					// limit outputs to valid range
+
+					// never go below min
+					if (outputs.output[i] < -1.0f) {
+						outputs.output[i] = -1.0f;
+					}
+
+					// never go below max
+					if (outputs.output[i] > 1.0f) {
+						outputs.output[i] = 1.0f;
+					}
+				}
+
+				// Output to the bus
+				_esc_controller.update_outputs(outputs.output, outputs.noutputs);
+			}
+
+		}
+
+		// Check arming state
+		bool updated = false;
+		orb_check(_armed_sub, &updated);
+
+		if (updated) {
+			orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed);
+
+			// Update the armed status and check that we're not locked down
+			bool set_armed = _armed.armed && !_armed.lockdown;
+
+			arm_actuators(set_armed);
+		}
+	}
+
+	teardown();
+	warnx("exiting.");
+
+	exit(0);
+}
+
+int
+UavcanNode::control_callback(uintptr_t handle,
+			 uint8_t control_group,
+			 uint8_t control_index,
+			 float &input)
+{
+	const actuator_controls_s *controls = (actuator_controls_s *)handle;
+
+	input = controls[control_group].control[control_index];
+	return 0;
+}
+
+int
+UavcanNode::teardown()
+{
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		if (_control_subs[i] > 0) {
+			::close(_control_subs[i]);
+			_control_subs[i] = -1;
+		}
+	}
+	return ::close(_armed_sub);
+}
+
+int
+UavcanNode::arm_actuators(bool arm)
+{
+	_is_armed = arm;
+	_esc_controller.arm_esc(arm);
+	return OK;
+}
+
+void
+UavcanNode::subscribe()
+{
+	// Subscribe/unsubscribe to required actuator control groups
+	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
+	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
+	_poll_fds_num = 0;
+	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
+		if (sub_groups & (1 << i)) {
+			warnx("subscribe to actuator_controls_%d", i);
+			_control_subs[i] = orb_subscribe(_control_topics[i]);
+		}
+		if (unsub_groups & (1 << i)) {
+			warnx("unsubscribe from actuator_controls_%d", i);
+			::close(_control_subs[i]);
+			_control_subs[i] = -1;
+		}
+
+		if (_control_subs[i] > 0) {
+			_poll_fds[_poll_fds_num].fd = _control_subs[i];
+			_poll_fds[_poll_fds_num].events = POLLIN;
+			_poll_fds_num++;
+		}
+	}
+}
+
+int
+UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
+{
+	int ret = OK;
+
+	lock();
+
+	switch (cmd) {
+	case PWM_SERVO_ARM:
+		arm_actuators(true);
+		break;
+
+	case PWM_SERVO_SET_ARM_OK:
+	case PWM_SERVO_CLEAR_ARM_OK:
+	case PWM_SERVO_SET_FORCE_SAFETY_OFF:
+		// these are no-ops, as no safety switch
+		break;
+
+	case PWM_SERVO_DISARM:
+		arm_actuators(false);
+		break;
+
+	case MIXERIOCGETOUTPUTCOUNT:
+		*(unsigned *)arg = _output_count;
+		break;
+
+	case MIXERIOCRESET:
+		if (_mixers != nullptr) {
+			delete _mixers;
+			_mixers = nullptr;
+			_groups_required = 0;
+		}
+
+		break;
+
+	case MIXERIOCLOADBUF: {
+			const char *buf = (const char *)arg;
+			unsigned buflen = strnlen(buf, 1024);
+
+			if (_mixers == nullptr)
+				_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
+
+			if (_mixers == nullptr) {
+				_groups_required = 0;
+				ret = -ENOMEM;
+
+			} else {
+
+				ret = _mixers->load_from_buf(buf, buflen);
+
+				if (ret != 0) {
+					warnx("mixer load failed with %d", ret);
+					delete _mixers;
+					_mixers = nullptr;
+					_groups_required = 0;
+					ret = -EINVAL;
+				} else {
+
+					_mixers->groups_required(_groups_required);
+				}
+			}
+
+			break;
+		}
+
+	default:
+		ret = -ENOTTY;
+		break;
+	}
+
+	unlock();
+
+	if (ret == -ENOTTY) {
+		ret = CDev::ioctl(filp, cmd, arg);
+	}
+
+	return ret;
+}
+
+void
+UavcanNode::print_info()
+{
+	if (!_instance) {
+		warnx("not running, start first");
+	}
+
+	warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+	warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
+}
+
+/*
+ * App entry point
+ */
+static void print_usage()
+{
+	warnx("usage: uavcan start <node_id> [can_bitrate]");
+}
+
+extern "C" __EXPORT int uavcan_main(int argc, char *argv[]);
+
+int uavcan_main(int argc, char *argv[])
+{
+	constexpr unsigned DEFAULT_CAN_BITRATE = 1000000;
+
+	if (argc < 2) {
+		print_usage();
+		::exit(1);
+	}
+
+	if (!std::strcmp(argv[1], "start")) {
+		if (argc < 3) {
+			print_usage();
+			::exit(1);
+		}
+
+		/*
+		 * Node ID
+		 */
+		const int node_id = atoi(argv[2]);
+
+		if (node_id < 0 || node_id > uavcan::NodeID::Max || !uavcan::NodeID(node_id).isUnicast()) {
+			warnx("Invalid Node ID %i", node_id);
+			::exit(1);
+		}
+
+		/*
+		 * CAN bitrate
+		 */
+		unsigned bitrate = 0;
+
+		if (argc > 3) {
+			bitrate = atol(argv[3]);
+		}
+
+		if (bitrate <= 0) {
+			bitrate = DEFAULT_CAN_BITRATE;
+		}
+
+		if (UavcanNode::instance()) {
+			errx(1, "already started");
+		}
+
+		/*
+		 * Start
+		 */
+		warnx("Node ID %u, bitrate %u", node_id, bitrate);
+		return UavcanNode::start(node_id, bitrate);
+
+	}
+
+	/* commands below require the app to be started */
+	UavcanNode *inst = UavcanNode::instance();
+
+	if (!inst) {
+		errx(1, "application not running");
+	}
+
+	if (!std::strcmp(argv[1], "status") || !std::strcmp(argv[1], "info")) {
+		
+			inst->print_info();
+			return OK;
+	}
+
+	if (!std::strcmp(argv[1], "stop")) {
+		
+			delete inst;
+			inst = nullptr;
+			return OK;
+	}
+
+	print_usage();
+	::exit(1);
+}
diff --git a/src/modules/uavcan/uavcan_main.hpp b/src/modules/uavcan/uavcan_main.hpp
new file mode 100644
index 000000000..443525379
--- /dev/null
+++ b/src/modules/uavcan/uavcan_main.hpp
@@ -0,0 +1,123 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2014 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.
+ *
+ ****************************************************************************/
+
+#pragma once
+
+#include <nuttx/config.h>
+
+#include <uavcan_stm32/uavcan_stm32.hpp>
+#include <drivers/device/device.h>
+
+#include <uORB/topics/actuator_controls.h>
+#include <uORB/topics/actuator_outputs.h>
+#include <uORB/topics/actuator_armed.h>
+
+#include "esc_controller.hpp"
+#include "gnss_receiver.hpp"
+
+/**
+ * @file uavcan_main.hpp
+ *
+ * Defines basic functinality of UAVCAN node.
+ *
+ * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ */
+
+#define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN	4
+#define UAVCAN_DEVICE_PATH	"/dev/uavcan/esc"
+
+/**
+ * A UAVCAN node.
+ */
+class UavcanNode : public device::CDev
+{
+	static constexpr unsigned MemPoolSize        = 10752;
+	static constexpr unsigned RxQueueLenPerIface = 64;
+	static constexpr unsigned StackSize          = 3000;
+
+public:
+	typedef uavcan::Node<MemPoolSize> Node;
+	typedef uavcan_stm32::CanInitHelper<RxQueueLenPerIface> CanInitHelper;
+
+	UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock);
+
+	virtual		~UavcanNode();
+
+	virtual int	ioctl(file *filp, int cmd, unsigned long arg);
+
+	static int	start(uavcan::NodeID node_id, uint32_t bitrate);
+
+	Node&		getNode() { return _node; }
+
+	static int	control_callback(uintptr_t handle,
+					 uint8_t control_group,
+					 uint8_t control_index,
+					 float &input);
+
+	void		subscribe();
+
+	int		teardown();
+	int		arm_actuators(bool arm);
+
+	void		print_info();
+
+	static UavcanNode* instance() { return _instance; }
+
+private:
+	int		init(uavcan::NodeID node_id);
+	void		node_spin_once();
+	int		run();
+
+	int			_task = -1;			///< handle to the OS task
+	bool			_task_should_exit = false;	///< flag to indicate to tear down the CAN driver
+	int			_armed_sub = -1;		///< uORB subscription of the arming status
+	actuator_armed_s	_armed;				///< the arming request of the system
+	bool			_is_armed = false;		///< the arming status of the actuators on the bus
+
+	unsigned		_output_count = 0;		///< number of actuators currently available
+
+	static UavcanNode	*_instance;			///< singleton pointer
+	Node			_node;				///< library instance
+	UavcanEscController	_esc_controller;
+	UavcanGnssReceiver 	_gnss_receiver;
+
+	MixerGroup		*_mixers = nullptr;
+
+	uint32_t		_groups_required = 0;
+	uint32_t		_groups_subscribed = 0;
+	int			_control_subs[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+	actuator_controls_s 	_controls[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+	orb_id_t		_control_topics[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN] = {};
+	pollfd			_poll_fds[NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN + 1] = {};	///< +1 for /dev/uavcan/busevent
+	unsigned		_poll_fds_num = 0;
+};
diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c
index e0e6ca537..c8d698b86 100644
--- a/src/systemcmds/pwm/pwm.c
+++ b/src/systemcmds/pwm/pwm.c
@@ -70,7 +70,7 @@ usage(const char *reason)
 {
 	if (reason != NULL)
 		warnx("%s", reason);
-	errx(1, 
+	errx(1,
 		"usage:\n"
 		"pwm arm|disarm|rate|failsafe|disarmed|min|max|test|info  ...\n"
 		"\n"
@@ -635,8 +635,28 @@ pwm_main(int argc, char *argv[])
 		}
 		exit(0);
 
+	} else if (!strcmp(argv[1], "forcefail")) {
+
+		if (argc < 3) {
+			errx(1, "arg missing [on|off]");
+		} else {
+
+			if (!strcmp(argv[2], "on")) {
+				/* force failsafe */
+				ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 1);
+			} else {
+				/* force failsafe */
+				ret = ioctl(fd, PWM_SERVO_SET_FORCE_FAILSAFE, 0);
+			}
+
+			if (ret != OK) {
+				warnx("FAILED setting forcefail %s", argv[2]);
+			}
+		}
+		exit(0);
 	}
-	usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info");
+
+	usage("specify arm|disarm|rate|failsafe|disarmed|min|max|test|info|forcefail");
 	return 0;
 }