Merge branch 'master' into mavlink_stack

45 files changed, 1197 insertions, 444 deletions

diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
index 2ec889efe..66a949af1 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -266,7 +266,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att);
 
 	int loopcounter = 0;
-	int printcounter = 0;
 
 	thread_running = true;
 
@@ -274,9 +273,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	// struct debug_key_value_s dbg = { .key = "", .value = 0.0f };
 	// orb_advert_t pub_dbg = -1;
 
-	float sensor_update_hz[3] = {0.0f, 0.0f, 0.0f};
-	// XXX write this out to perf regs
-
 	/* keep track of sensor updates */
 	uint64_t sensor_last_timestamp[3] = {0, 0, 0};
 
@@ -287,11 +283,9 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 	/* initialize parameter handles */
 	parameters_init(&ekf_param_handles);
 
-	uint64_t start_time = hrt_absolute_time();
 	bool initialized = false;
 
 	float gyro_offsets[3] = { 0.0f, 0.0f, 0.0f };
-	unsigned offset_count = 0;
 
 	/* rotation matrix for magnetic declination */
 	math::Matrix<3, 3> R_decl;
@@ -382,7 +376,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					/* Fill in gyro measurements */
 					if (sensor_last_timestamp[0] != raw.timestamp) {
 						update_vect[0] = 1;
-						sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
+						// sensor_update_hz[0] = 1e6f / (raw.timestamp - sensor_last_timestamp[0]);
 						sensor_last_timestamp[0] = raw.timestamp;
 					}
 
@@ -393,7 +387,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					/* update accelerometer measurements */
 					if (sensor_last_timestamp[1] != raw.accelerometer_timestamp) {
 						update_vect[1] = 1;
-						sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
+						// sensor_update_hz[1] = 1e6f / (raw.timestamp - sensor_last_timestamp[1]);
 						sensor_last_timestamp[1] = raw.accelerometer_timestamp;
 					}
 
@@ -445,7 +439,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					/* update magnetometer measurements */
 					if (sensor_last_timestamp[2] != raw.magnetometer_timestamp) {
 						update_vect[2] = 1;
-						sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
+						// sensor_update_hz[2] = 1e6f / (raw.timestamp - sensor_last_timestamp[2]);
 						sensor_last_timestamp[2] = raw.magnetometer_timestamp;
 					}
 
@@ -498,8 +492,6 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 						continue;
 					}
 
-					uint64_t timing_start = hrt_absolute_time();
-
 					attitudeKalmanfilter(update_vect, dt, z_k, x_aposteriori_k, P_aposteriori_k, ekf_params.q, ekf_params.r,
 							     euler, Rot_matrix, x_aposteriori, P_aposteriori);
 
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 13da27dcd..65922b2a5 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -453,6 +453,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 					/* AUTO */
 					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) {
+					/* ACRO */
+					main_res = main_state_transition(status, MAIN_STATE_ACRO);
 				}
 
 			} else {
@@ -652,6 +656,7 @@ int commander_thread_main(int argc, char *argv[])
 	main_states_str[1] = "ALTCTL";
 	main_states_str[2] = "POSCTL";
 	main_states_str[3] = "AUTO";
+	main_states_str[4] = "ACRO";
 
 	char *arming_states_str[ARMING_STATE_MAX];
 	arming_states_str[0] = "INIT";
@@ -1012,7 +1017,26 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* update condition_local_position_valid and condition_local_altitude_valid */
-		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed);
+		/* hysteresis for EPH */
+		bool local_eph_good;
+
+		if (status.condition_global_position_valid) {
+			if (local_position.eph > eph_epv_threshold * 2.0f) {
+				local_eph_good = false;
+
+			} else {
+				local_eph_good = true;
+			}
+
+		} else {
+			if (local_position.eph < eph_epv_threshold) {
+				local_eph_good = true;
+
+			} else {
+				local_eph_good = false;
+			}
+		}
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
 		static bool published_condition_landed_fw = false;
@@ -1187,7 +1211,7 @@ int commander_thread_main(int argc, char *argv[])
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
 			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-			    (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
+			    (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
 			    sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@@ -1209,10 +1233,10 @@ int commander_thread_main(int argc, char *argv[])
 			    sp_man.r > STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 				if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) {
 					if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) {
-						print_reject_arm("NOT ARMING: Press safety switch first.");
+						print_reject_arm("#audio: NOT ARMING: Press safety switch first.");
 
 					} else if (status.main_state != MAIN_STATE_MANUAL) {
-						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
+						print_reject_arm("#audio: NOT ARMING: Switch to MANUAL mode first.");
 
 					} else {
 						arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
@@ -1384,7 +1408,7 @@ int commander_thread_main(int argc, char *argv[])
 				home.alt = global_position.alt;
 
 				warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", home.lat, home.lon, (double)home.alt);
-				mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
+				mavlink_log_info(mavlink_fd, "home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);
 
 				/* announce new home position */
 				if (home_pub > 0) {
@@ -1600,7 +1624,12 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 		break;
 
 	case SWITCH_POS_OFF:		// MANUAL
-		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		if (sp_man->acro_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status, MAIN_STATE_ACRO);
+
+		} else {
+			res = main_state_transition(status, MAIN_STATE_MANUAL);
+		}
 		// TRANSITION_DENIED is not possible here
 		break;
 
@@ -1710,6 +1739,18 @@ set_control_mode()
 
 		case MAIN_STATE_AUTO:
 			navigator_enabled = true;
+			break;
+
+		case MAIN_STATE_ACRO:
+			control_mode.flag_control_manual_enabled = true;
+			control_mode.flag_control_auto_enabled = false;
+			control_mode.flag_control_rates_enabled = true;
+			control_mode.flag_control_attitude_enabled = false;
+			control_mode.flag_control_altitude_enabled = false;
+			control_mode.flag_control_climb_rate_enabled = false;
+			control_mode.flag_control_position_enabled = false;
+			control_mode.flag_control_velocity_enabled = false;
+			break;
 
 		default:
 			break;
@@ -1807,7 +1848,8 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 		break;
 
 	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
-		mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
+		/* this needs additional hints to the user - so let other messages pass and be spoken */
+		mavlink_log_critical(mavlink_fd, "command temporarily rejected: %u", cmd.command);
 		tune_negative(true);
 		break;
 
diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index a83c81850..e0f8dc95d 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -15,6 +15,7 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_ALTCTL,
 	PX4_CUSTOM_MAIN_MODE_POSCTL,
 	PX4_CUSTOM_MAIN_MODE_AUTO,
+	PX4_CUSTOM_MAIN_MODE_ACRO,
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index abcf72717..87309b238 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -138,7 +138,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 				//      Allow if HIL_STATE_ON
 				if (status->arming_state != ARMING_STATE_IN_AIR_RESTORE && status->hil_state == HIL_STATE_OFF && safety->safety_switch_available && !safety->safety_off) {
 					if (mavlink_fd) {
-						mavlink_log_critical(mavlink_fd, "NOT ARMING: Press safety switch first.");
+						mavlink_log_critical(mavlink_fd, "#audio: NOT ARMING: Press safety switch first.");
 					}
 
 					valid_transition = false;
@@ -222,6 +222,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		ret = TRANSITION_CHANGED;
 		break;
 
+	case MAIN_STATE_ACRO:
+		ret = TRANSITION_CHANGED;
+		break;
+
 	case MAIN_STATE_ALTCTL:
 
 		/* need at minimum altitude estimate */
@@ -308,7 +312,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 		case HIL_STATE_OFF:
 
 			/* we're in HIL and unexpected things can happen if we disable HIL now */
-			mavlink_log_critical(mavlink_fd, "Not switching off HIL (safety)");
+			mavlink_log_critical(mavlink_fd, "#audio: Not switching off HIL (safety)");
 			valid_transition = false;
 
 			break;
diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 907f4c2e1..0a6d3fa8f 100644
--- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file fw_att_pos_estimator_main.cpp
+ * @file ekf_att_pos_estimator_main.cpp
  * Implementation of the attitude and position estimator.
  *
  * @author Paul Riseborough <p_riseborough@live.com.au>
@@ -336,13 +336,13 @@ FixedwingEstimator::FixedwingEstimator() :
 	_baro_gps_offset(0.0f),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_COUNT, "fw_att_pos_estimator")),
-	_perf_gyro(perf_alloc(PC_COUNT, "fw_ekf_gyro_upd")),
-	_perf_accel(perf_alloc(PC_COUNT, "fw_ekf_accel_upd")),
-	_perf_mag(perf_alloc(PC_COUNT, "fw_ekf_mag_upd")),
-	_perf_gps(perf_alloc(PC_COUNT, "fw_ekf_gps_upd")),
-	_perf_baro(perf_alloc(PC_COUNT, "fw_ekf_baro_upd")),
-	_perf_airspeed(perf_alloc(PC_COUNT, "fw_ekf_aspd_upd")),
+	_loop_perf(perf_alloc(PC_COUNT, "ekf_att_pos_estimator")),
+	_perf_gyro(perf_alloc(PC_COUNT, "ekf_att_pos_gyro_upd")),
+	_perf_accel(perf_alloc(PC_COUNT, "ekf_att_pos_accel_upd")),
+	_perf_mag(perf_alloc(PC_COUNT, "ekf_att_pos_mag_upd")),
+	_perf_gps(perf_alloc(PC_COUNT, "ekf_att_pos_gps_upd")),
+	_perf_baro(perf_alloc(PC_COUNT, "ekf_att_pos_baro_upd")),
+	_perf_airspeed(perf_alloc(PC_COUNT, "ekf_att_pos_aspd_upd")),
 
 /* states */
 	_initialized(false),
@@ -1067,7 +1067,7 @@ FixedwingEstimator::task_main()
 					mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
 					warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
 						(double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
-					warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", _ekf->baroHgt, _baro_ref, _baro_gps_offset);
+					warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref, (double)_baro_gps_offset);
 					warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m, (double)math::degrees(declination));
 
 					_gps_initialized = true;
diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
index d2c6e1f15..8c82dd6c1 100644
--- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file fw_att_pos_estimator_params.c
+ * @file ekf_att_pos_estimator_params.c
  *
  * Parameters defined by the attitude and position estimator task
  *
@@ -226,11 +226,11 @@ PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f);
  * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f.
  * Increasing this value makes the bias estimation faster and noisier.
  *
- * @min 0.0001
+ * @min 0.00001
  * @max 0.001
  * @group Position Estimator
  */
-PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f);
+PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.00005f);
 
 /**
  * Magnetometer earth frame offsets process noise
diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp
index 23ecd89ac..5db1adbb3 100644
--- a/src/modules/ekf_att_pos_estimator/estimator.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator.cpp
@@ -5,7 +5,7 @@
 // Define EKF_DEBUG here to enable the debug print calls
 // if the macro is not set, these will be completely
 // optimized out by the compiler.
-#define EKF_DEBUG
+//#define EKF_DEBUG
 
 #ifdef EKF_DEBUG
 #include <stdio.h>
diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk
index 30955d0dd..6fefec2c2 100644
--- a/src/modules/ekf_att_pos_estimator/module.mk
+++ b/src/modules/ekf_att_pos_estimator/module.mk
@@ -32,11 +32,11 @@
 ############################################################################
 
 #
-# Main Attitude and Position Estimator for Fixed Wing Aircraft
+# Main EKF Attitude and Position Estimator
 #
 
 MODULE_COMMAND	= ekf_att_pos_estimator
 
-SRCS		= fw_att_pos_estimator_main.cpp \
-		  fw_att_pos_estimator_params.c \
+SRCS		= ekf_att_pos_estimator_main.cpp \
+		  ekf_att_pos_estimator_params.c \
 		  estimator.cpp
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index 1f3e9f098..178b590ae 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -134,6 +134,8 @@ private:
 	struct vehicle_global_position_s		_global_pos;		/**< global position */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
+	perf_counter_t	_nonfinite_input_perf;		/**< performance counter for non finite input */
+	perf_counter_t	_nonfinite_output_perf;		/**< performance counter for non finite output */
 
 	bool		_setpoint_valid;		/**< flag if the position control setpoint is valid */
 
@@ -304,12 +306,14 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 
 /* publications */
 	_rate_sp_pub(-1),
-	_actuators_0_pub(-1),
 	_attitude_sp_pub(-1),
+	_actuators_0_pub(-1),
 	_actuators_1_pub(-1),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
+	_nonfinite_input_perf(perf_alloc(PC_COUNT, "fw att control nonfinite input")),
+	_nonfinite_output_perf(perf_alloc(PC_COUNT, "fw att control nonfinite output")),
 /* states */
 	_setpoint_valid(false)
 {
@@ -387,6 +391,10 @@ FixedwingAttitudeControl::~FixedwingAttitudeControl()
 		} while (_control_task != -1);
 	}
 
+	perf_free(_loop_perf);
+	perf_free(_nonfinite_input_perf);
+	perf_free(_nonfinite_output_perf);
+
 	att_control::g_control = nullptr;
 }
 
@@ -592,6 +600,8 @@ FixedwingAttitudeControl::task_main()
 
 	while (!_task_should_exit) {
 
+		static int loop_counter = 0;
+
 		/* wait for up to 500ms for data */
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 100);
 
@@ -672,10 +682,12 @@ FixedwingAttitudeControl::task_main()
 				float airspeed;
 
 				/* if airspeed is not updating, we assume the normal average speed */
-				if (!isfinite(_airspeed.true_airspeed_m_s) ||
+				if (bool nonfinite = !isfinite(_airspeed.true_airspeed_m_s) ||
 				    hrt_elapsed_time(&_airspeed.timestamp) > 1e6) {
 					airspeed = _parameters.airspeed_trim;
-
+					if (nonfinite) {
+						perf_count(_nonfinite_input_perf);
+					}
 				} else {
 					airspeed = _airspeed.true_airspeed_m_s;
 				}
@@ -755,7 +767,9 @@ FixedwingAttitudeControl::task_main()
 					speed_body_v = _att.R[0][1] * _global_pos.vel_n + _att.R[1][1] * _global_pos.vel_e + _att.R[2][1] * _global_pos.vel_d;
 					speed_body_w = _att.R[0][2] * _global_pos.vel_n + _att.R[1][2] * _global_pos.vel_e + _att.R[2][2] * _global_pos.vel_d;
 				} else	{
-					warnx("Did not get a valid R\n");
+					if (loop_counter % 10 == 0) {
+						warnx("Did not get a valid R\n");
+					}
 				}
 
 				/* Run attitude controllers */
@@ -773,7 +787,12 @@ FixedwingAttitudeControl::task_main()
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
 					_actuators.control[0] = (isfinite(roll_u)) ? roll_u + _parameters.trim_roll : _parameters.trim_roll;
 					if (!isfinite(roll_u)) {
-						warnx("roll_u %.4f", roll_u);
+						_roll_ctrl.reset_integrator();
+						perf_count(_nonfinite_output_perf);
+
+						if (loop_counter % 10 == 0) {
+							warnx("roll_u %.4f", (double)roll_u);
+						}
 					}
 
 					float pitch_u = _pitch_ctrl.control_bodyrate(_att.roll, _att.pitch,
@@ -782,8 +801,22 @@ FixedwingAttitudeControl::task_main()
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
 					_actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch;
 					if (!isfinite(pitch_u)) {
-						warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f",
-								(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body);
+						_pitch_ctrl.reset_integrator();
+						perf_count(_nonfinite_output_perf);
+						if (loop_counter % 10 == 0) {
+							warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f,"
+								" airspeed %.4f, airspeed_scaling %.4f,"
+								" roll_sp %.4f, pitch_sp %.4f,"
+								" _roll_ctrl.get_desired_rate() %.4f,"
+								" _pitch_ctrl.get_desired_rate() %.4f"
+								" att_sp.roll_body %.4f",
+								(double)pitch_u, (double)_yaw_ctrl.get_desired_rate(),
+								(double)airspeed, (double)airspeed_scaling,
+								(double)roll_sp, (double)pitch_sp,
+								(double)_roll_ctrl.get_desired_rate(),
+								(double)_pitch_ctrl.get_desired_rate(),
+								(double)_att_sp.roll_body);
+						}
 					}
 
 					float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch,
@@ -792,16 +825,25 @@ FixedwingAttitudeControl::task_main()
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
 					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
 					if (!isfinite(yaw_u)) {
-						warnx("yaw_u %.4f", (double)yaw_u);
+						_yaw_ctrl.reset_integrator();
+						perf_count(_nonfinite_output_perf);
+						if (loop_counter % 10 == 0) {
+							warnx("yaw_u %.4f", (double)yaw_u);
+						}
 					}
 
 					/* throttle passed through */
 					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
 					if (!isfinite(throttle_sp)) {
-						warnx("throttle_sp %.4f", (double)throttle_sp);
+						if (loop_counter % 10 == 0) {
+							warnx("throttle_sp %.4f", (double)throttle_sp);
+						}
 					}
 				} else {
-					warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
+					perf_count(_nonfinite_input_perf);
+					if (loop_counter % 10 == 0) {
+						warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp);
+					}
 				}
 
 				/*
@@ -865,6 +907,7 @@ FixedwingAttitudeControl::task_main()
 
 		}
 
+		loop_counter++;
 		perf_end(_loop_perf);
 	}
 
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index aa637e207..7cae84678 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -50,149 +50,322 @@ f *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  * Controller parameters, accessible via MAVLink
  *
  */
-// @DisplayName		Attitude Time Constant
-// @Description		This defines the latency between a step input and the achieved setpoint (inverse to a P gain). Half a second is a good start value and fits for most average systems. Smaller systems may require smaller values, but as this will wear out servos faster, the value should only be decreased as needed.
-// @Range		0.4 to 1.0 seconds, in tens of seconds
+
+/**
+ * Attitude Time Constant
+ *
+ * This defines the latency between a step input and the achieved setpoint
+ * (inverse to a P gain). Half a second is a good start value and fits for
+ * most average systems. Smaller systems may require smaller values, but as
+ * this will wear out servos faster, the value should only be decreased as
+ * needed.
+ *
+ * @unit seconds
+ * @min 0.4
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_ATT_TC, 0.5f);
 
-// @DisplayName		Pitch rate proportional gain.
-// @Description		This defines how much the elevator input will be commanded depending on the current body angular rate error.
-// @Range		10 to 200, 1 increments
+/**
+ * Pitch rate proportional gain.
+ *
+ * This defines how much the elevator input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PR_P, 0.05f);
 
-// @DisplayName		Pitch rate integrator gain.
-// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
-// @Range		0 to 50.0
+/**
+ * Pitch rate integrator gain.
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 50.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PR_I, 0.0f);
 
-// @DisplayName		Maximum positive / up pitch rate.
-// @Description		This limits the maximum pitch up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds, in 1 increments
+/**
+ * Maximum positive / up pitch rate.
+ *
+ * This limits the maximum pitch up angular rate the controller will output (in
+ * degrees per second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_RMAX_POS, 0.0f);
 
-// @DisplayName		Maximum negative / down pitch rate.
-// @Description		This limits the maximum pitch down up angular rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds, in 1 increments
+/**
+ * Maximum negative / down pitch rate.
+ *
+ * This limits the maximum pitch down up angular rate the controller will
+ * output (in degrees per second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_RMAX_NEG, 0.0f);
 
-// @DisplayName		Pitch rate integrator limit
-// @Description		The portion of the integrator part in the control surface deflection is limited to this value
-// @Range		0.0 to 1
-// @Increment		0.1
+/**
+ * Pitch rate integrator limit
+ *
+ * The portion of the integrator part in the control surface deflection is
+ * limited to this value
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PR_IMAX, 0.2f);
 
-// @DisplayName		Roll to Pitch feedforward gain.
-// @Description		This compensates during turns and ensures the nose stays level.
-// @Range		0.5 2.0
-// @Increment		0.05
-// @User		User
+/**
+ * Roll to Pitch feedforward gain.
+ *
+ * This compensates during turns and ensures the nose stays level.
+ *
+ * @min 0.0
+ * @max 2.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_P_ROLLFF, 0.0f); //xxx: set to 0 as default, see comment in ECL_PitchController::control_attitude (float turn_offset = ...)
 
-// @DisplayName		Roll rate proportional Gain.
-// @Description		This defines how much the aileron input will be commanded depending on the current body angular rate error.
-// @Range		10.0 200.0
-// @Increment		10.0
-// @User		User
+/**
+ * Roll rate proportional Gain
+ *
+ * This defines how much the aileron input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RR_P, 0.05f);
 
-// @DisplayName		Roll rate integrator Gain
-// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
-// @Range		0.0 100.0
-// @Increment		5.0
-// @User		User
+/**
+ * Roll rate integrator Gain
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 100.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RR_I, 0.0f);
 
-// @DisplayName		Roll Integrator Anti-Windup
-// @Description		The portion of the integrator part in the control surface deflection is limited to this value.
-// @Range		0.0 to 1.0
-// @Increment		0.1
+/**
+ * Roll Integrator Anti-Windup
+ *
+ * The portion of the integrator part in the control surface deflection is limited to this value.
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RR_IMAX, 0.2f);
 
-// @DisplayName		Maximum Roll Rate
-// @Description		This limits the maximum roll rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds
-// @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_R_RMAX, 0);
+/**
+ * Maximum Roll Rate
+ *
+ * This limits the maximum roll rate the controller will output (in degrees per
+ * second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_R_RMAX, 0.0f);
 
-// @DisplayName		Yaw rate proportional gain.
-// @Description		This defines how much the rudder input will be commanded depending on the current body angular rate error.
-// @Range		10 to 200, 1 increments
-PARAM_DEFINE_FLOAT(FW_YR_P, 0.05);
+/**
+ * Yaw rate proportional gain
+ *
+ * This defines how much the rudder input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_YR_P, 0.05f);
 
-// @DisplayName		Yaw rate integrator gain.
-// @Description		This gain defines how much control response will result out of a steady state error. It trims any constant error.
-// @Range		0 to 50.0
+/**
+ * Yaw rate integrator gain
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 50.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_YR_I, 0.0f);
 
-// @DisplayName		Yaw rate integrator limit
-// @Description		The portion of the integrator part in the control surface deflection is limited to this value
-// @Range		0.0 to 1
-// @Increment		0.1
+/**
+ * Yaw rate integrator limit
+ *
+ * The portion of the integrator part in the control surface deflection is
+ * limited to this value
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
 
-// @DisplayName		Maximum Yaw Rate
-// @Description		This limits the maximum yaw rate the controller will output (in degrees per second). Setting a value of zero disables the limit.
-// @Range		0 to 90.0 degrees per seconds
-// @Increment		1.0
-PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0);
+/**
+ * Maximum Yaw Rate
+ *
+ * This limits the maximum yaw rate the controller will output (in degrees per
+ * second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
 
-// @DisplayName		Roll rate feed forward
-// @Description		Direct feed forward from rate setpoint to control surface output
-// @Range		0 to 10
-// @Increment		0.1
+/**
+ * Roll rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RR_FF, 0.3f);
 
-// @DisplayName		Pitch rate feed forward
-// @Description		Direct feed forward from rate setpoint to control surface output
-// @Range		0 to 10
-// @Increment		0.1
+/**
+ * Pitch rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PR_FF, 0.4f);
 
-// @DisplayName		Yaw rate feed forward
-// @Description		Direct feed forward from rate setpoint to control surface output
-// @Range		0 to 10
-// @Increment		0.1
+/**
+ * Yaw rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
 
-// @DisplayName		Minimal speed for yaw coordination
-// @Description		For airspeeds above this value the yaw rate is calculated for a coordinated turn. Set to a very high value to disable.
-// @Range		0 to 90.0 degrees per seconds
-// @Increment		1.0
+/**
+ * Minimal speed for yaw coordination
+ *
+ * For airspeeds above this value, the yaw rate is calculated for a coordinated
+ * turn. Set to a very high value to disable.
+ *
+ * @unit m/s
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
 
-/* Airspeed parameters: the following parameters about airspeed are used by the attitude and the positon controller */
+/* Airspeed parameters:
+ * The following parameters about airspeed are used by the attitude and the
+ * position controller.
+ * */
 
-// @DisplayName		Minimum Airspeed
-// @Description		If the airspeed falls below this value the TECS controller will try to increase airspeed more aggressively
-// @Range		0.0 to 30
+/**
+ * Minimum Airspeed
+ *
+ * If the airspeed falls below this value, the TECS controller will try to
+ * increase airspeed more aggressively.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MIN, 13.0f);
 
-// @DisplayName		Trim Airspeed
-// @Description		The TECS controller tries to fly at this airspeed
-// @Range		0.0 to 30
+/**
+ * Trim Airspeed
+ *
+ * The TECS controller tries to fly at this airspeed.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_TRIM, 20.0f);
 
-// @DisplayName		Maximum Airspeed
-// @Description		If the airspeed is above this value the TECS controller will try to decrease airspeed more aggressively
-// @Range		0.0 to 30
+/**
+ * Maximum Airspeed
+ *
+ * If the airspeed is above this value, the TECS controller will try to decrease
+ * airspeed more aggressively.
+ *
+ * @unit m/s
+ * @min 0.0
+ * @max 30.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_AIRSPD_MAX, 50.0f);
 
-// @DisplayName		Roll Setpoint Offset
-// @Description		An airframe specific offset of the roll setpoint in degrees, the value is added to the roll setpoint and should correspond to the typical cruise speed of the airframe
-// @Range		-90.0 to 90.0
+/**
+ * Roll Setpoint Offset
+ *
+ * An airframe specific offset of the roll setpoint in degrees, the value is
+ * added to the roll setpoint and should correspond to the typical cruise speed
+ * of the airframe.
+ *
+ * @unit deg
+ * @min -90.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_RSP_OFF, 0.0f);
 
-// @DisplayName		Pitch Setpoint Offset
-// @Description		An airframe specific offset of the pitch setpoint in degrees, the value is added to the pitch setpoint and should correspond to the typical cruise speed of the airframe
-// @Range		-90.0 to 90.0
+/**
+ * Pitch Setpoint Offset
+ *
+ * An airframe specific offset of the pitch setpoint in degrees, the value is
+ * added to the pitch setpoint and should correspond to the typical cruise
+ * speed of the airframe.
+ *
+ * @unit deg
+ * @min -90.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_PSP_OFF, 0.0f);
 
-// @DisplayName		Max Manual Roll
-// @Description		Max roll for manual control in attitude stabilized mode
-// @Range		0.0 to 90.0
+/**
+ * Max Manual Roll
+ *
+ * Max roll for manual control in attitude stabilized mode
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_MAN_R_MAX, 45.0f);
 
-// @DisplayName		Max Manual Pitch
-// @Description		Max pitch for manual control in attitude stabilized mode
-// @Range		0.0 to 90.0
+/**
+ * Max Manual Pitch
+ *
+ * Max pitch for manual control in attitude stabilized mode
+ *
+ * @unit deg
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
 PARAM_DEFINE_FLOAT(FW_MAN_P_MAX, 45.0f);
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 9cbc26efe..116d3cc63 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
@@ -232,8 +232,6 @@ private:
 
 		float throttle_land_max;
 
-		float loiter_hold_radius;
-
 		float heightrate_p;
 		float speedrate_p;
 
@@ -277,8 +275,6 @@ private:
 
 		param_t throttle_land_max;
 
-		param_t loiter_hold_radius;
-
 		param_t heightrate_p;
 		param_t speedrate_p;
 
@@ -418,8 +414,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_motor_lim(false),
 	land_onslope(false),
 	launch_detected(false),
-	last_manual(false),
 	usePreTakeoffThrust(false),
+	last_manual(false),
 	flare_curve_alt_rel_last(0.0f),
 	launchDetector(),
 	_airspeed_error(0.0f),
@@ -441,7 +437,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
 	_parameter_handles.l1_damping = param_find("FW_L1_DAMPING");
-	_parameter_handles.loiter_hold_radius = param_find("FW_LOITER_R");
 
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
@@ -513,7 +508,6 @@ FixedwingPositionControl::parameters_update()
 	/* L1 control parameters */
 	param_get(_parameter_handles.l1_damping, &(_parameters.l1_damping));
 	param_get(_parameter_handles.l1_period, &(_parameters.l1_period));
-	param_get(_parameter_handles.loiter_hold_radius, &(_parameters.loiter_hold_radius));
 
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
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 f258f77da..52128e1b7 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
@@ -72,17 +72,6 @@ PARAM_DEFINE_FLOAT(FW_L1_PERIOD, 25.0f);
 PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
 
 /**
- * Default Loiter Radius
- *
- * This radius is used when no other loiter radius is set.
- *
- * @min 10.0
- * @max 100.0
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LOITER_R, 50.0f);
-
-/**
  * Cruise throttle
  *
  * This is the throttle setting required to achieve the desired cruise speed. Most airframes have a value of 0.5-0.7.
diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index e9932aac3..39610fdd8 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -105,7 +105,8 @@ static struct file_operations fops;
  */
 extern "C" __EXPORT int mavlink_main(int argc, char *argv[]);
 
-static uint64_t last_write_times[6] = {0};
+static uint64_t last_write_success_times[6] = {0};
+static uint64_t last_write_try_times[6] = {0};
 
 /*
  * Internal function to send the bytes through the right serial port
@@ -149,10 +150,7 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 		instance = Mavlink::get_instance(6);
 		break;
 #endif
-	}
-
-	/* no valid instance, bail */
-	if (!instance) {
+		default:
 		return;
 	}
 
@@ -169,38 +167,40 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 	if (instance->get_flow_control_enabled()
 		&& ioctl(uart, FIONWRITE, (unsigned long)&buf_free) == 0) {
 
-		if (buf_free == 0) {
-
-			if (last_write_times[(unsigned)channel] != 0 &&
-				hrt_elapsed_time(&last_write_times[(unsigned)channel]) > 500 * 1000UL) {
-
-				warnx("DISABLING HARDWARE FLOW CONTROL");
-				instance->enable_flow_control(false);
-			}
-
-		} else {
-
-			/* apparently there is space left, although we might be
-			 * partially overflooding the buffer already */
-			last_write_times[(unsigned)channel] = hrt_absolute_time();
+		/* 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])
+		{
+			warnx("DISABLING HARDWARE FLOW CONTROL");
+			instance->enable_flow_control(false);
 		}
+
 	}
 
 	/* If the wait until transmit flag is on, only transmit after we've received messages.
 	   Otherwise, transmit all the time. */
 	if (instance->should_transmit()) {
+		last_write_try_times[(unsigned)channel] = hrt_absolute_time();
 
 		/* check if there is space in the buffer, let it overflow else */
 		if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) {
 
-			if (desired > buf_free) {
-				desired = buf_free;
+			if (buf_free < desired) {
+				/* we don't want to send anything just in half, so return */
+				return;
 			}
 		}
 
 		ssize_t ret = write(uart, ch, desired);
 		if (ret != desired) {
 			warnx("TX FAIL");
+		} else {
+			last_write_success_times[(unsigned)channel] = last_write_try_times[(unsigned)channel];
 		}
 	}
 
@@ -211,9 +211,9 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length
 static void usage(void);
 
 Mavlink::Mavlink() :
-	next(nullptr),
 	_device_name(DEFAULT_DEVICE_NAME),
 	_task_should_exit(false),
+	next(nullptr),
 	_mavlink_fd(-1),
 	_task_running(false),
 	_hil_enabled(false),
@@ -225,6 +225,8 @@ Mavlink::Mavlink() :
 	_subscriptions(nullptr),
 	_streams(nullptr),
 	_mission_pub(-1),
+	_mode(MAVLINK_MODE_NORMAL),
+	_total_counter(0),
 	_verbose(false),
 	_forwarding_on(false),
 	_passing_on(false),
@@ -424,7 +426,7 @@ Mavlink::instance_exists(const char *device_name, Mavlink *self)
 void
 Mavlink::forward_message(mavlink_message_t *msg, Mavlink *self)
 {
-	
+
 	Mavlink *inst;
 	LL_FOREACH(_mavlink_instances, inst) {
 		if (inst != self) {
@@ -789,9 +791,14 @@ void Mavlink::mavlink_pm_message_handler(const mavlink_channel_t chan, const mav
 {
 	switch (msg->msgid) {
 	case MAVLINK_MSG_ID_PARAM_REQUEST_LIST: {
-			/* Start sending parameters */
-			mavlink_pm_start_queued_send();
-			mavlink_missionlib_send_gcs_string("[pm] sending 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)) {
+				/* Start sending parameters */
+				mavlink_pm_start_queued_send();
+				mavlink_missionlib_send_gcs_string("[mavlink pm] sending list");
+			}
 		} break;
 
 	case MAVLINK_MSG_ID_PARAM_SET: {
@@ -890,11 +897,12 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 
 	switch (mavlink_mission_item->command) {
 	case MAV_CMD_NAV_TAKEOFF:
-		mission_item->pitch_min = mavlink_mission_item->param2;
+		mission_item->pitch_min = mavlink_mission_item->param1;
 		break;
 
 	default:
 		mission_item->acceptance_radius = mavlink_mission_item->param2;
+		mission_item->time_inside = mavlink_mission_item->param1;
 		break;
 	}
 
@@ -903,7 +911,6 @@ int Mavlink::map_mavlink_mission_item_to_mission_item(const mavlink_mission_item
 	mission_item->loiter_direction = (mavlink_mission_item->param3 > 0) ? 1 : -1; /* 1 if positive CW, -1 if negative CCW */
 	mission_item->nav_cmd = (NAV_CMD)mavlink_mission_item->command;
 
-	mission_item->time_inside = mavlink_mission_item->param1;
 	mission_item->autocontinue = mavlink_mission_item->autocontinue;
 	// mission_item->index = mavlink_mission_item->seq;
 	mission_item->origin = ORIGIN_MAVLINK;
@@ -922,11 +929,12 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
 
 	switch (mission_item->nav_cmd) {
 	case NAV_CMD_TAKEOFF:
-		mavlink_mission_item->param2 = mission_item->pitch_min;
+		mavlink_mission_item->param1 = mission_item->pitch_min;
 		break;
 
 	default:
 		mavlink_mission_item->param2 = mission_item->acceptance_radius;
+		mavlink_mission_item->param1 = mission_item->time_inside;
 		break;
 	}
 
@@ -937,7 +945,6 @@ int Mavlink::map_mission_item_to_mavlink_mission_item(const struct mission_item_
 	mavlink_mission_item->param4 = mission_item->yaw * M_RAD_TO_DEG_F;
 	mavlink_mission_item->param3 = mission_item->loiter_radius * (float)mission_item->loiter_direction;
 	mavlink_mission_item->command = mission_item->nav_cmd;
-	mavlink_mission_item->param1 = mission_item->time_inside;
 	mavlink_mission_item->autocontinue = mission_item->autocontinue;
 	// mavlink_mission_item->seq = mission_item->index;
 
@@ -953,6 +960,7 @@ void Mavlink::mavlink_wpm_init(mavlink_wpm_storage *state)
 	state->current_partner_compid = 0;
 	state->timestamp_lastaction = 0;
 	state->timestamp_last_send_setpoint = 0;
+	state->timestamp_last_send_request = 0;
 	state->timeout = MAVLINK_WPM_PROTOCOL_TIMEOUT_DEFAULT;
 	state->current_dataman_id = 0;
 }
@@ -1051,6 +1059,7 @@ void Mavlink::mavlink_wpm_send_waypoint(uint8_t sysid, uint8_t compid, uint16_t
 
 	} else {
 		mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
+		mavlink_missionlib_send_gcs_string("#audio: Unable to read from micro SD");
 
 		if (_verbose) { warnx("ERROR: could not read WP%u", seq); }
 	}
@@ -1066,6 +1075,7 @@ void Mavlink::mavlink_wpm_send_waypoint_request(uint8_t sysid, uint8_t compid, u
 		wpr.seq = seq;
 		mavlink_msg_mission_request_encode_chan(mavlink_system.sysid, _mavlink_wpm_comp_id, _channel, &msg, &wpr);
 		mavlink_missionlib_send_message(&msg);
+		_wpm->timestamp_last_send_request = hrt_absolute_time();
 
 		if (_verbose) { warnx("Sent waypoint request %u to ID %u", wpr.seq, wpr.target_system); }
 
@@ -1104,6 +1114,10 @@ void Mavlink::mavlink_waypoint_eventloop(uint64_t now)
 		_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 		_wpm->current_partner_sysid = 0;
 		_wpm->current_partner_compid = 0;
+
+	} else if (now - _wpm->timestamp_last_send_request > 500000 && _wpm->current_state == MAVLINK_WPM_STATE_GETLIST_GETWPS) {
+		/* try to get WP again after short timeout */
+		mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
 	}
 }
 
@@ -1159,9 +1173,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN WP CURR CMD: Busy");
 				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
 			}
 
 			break;
@@ -1192,9 +1203,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN REQUEST LIST: Busy");
 				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. REQUEST LIST: target id mismatch");
 			}
 
 			break;
@@ -1274,9 +1282,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					mavlink_missionlib_send_gcs_string("REJ. WP CMD: Busy");
 
-				} else {
-
-					mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
+					if (_verbose) { warnx("Ignored MAVLINK_MSG_ID_MISSION_ITEM_REQUEST from ID %u because i'm already talking to ID %u.", msg->sysid, _wpm->current_partner_sysid); }
 				}
 			}
 
@@ -1325,10 +1331,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				} else {
 					mavlink_missionlib_send_gcs_string("IGN MISSION_COUNT CMD: Busy");
 				}
-
-			} else {
-
-				mavlink_missionlib_send_gcs_string("REJ. WP COUNT CMD: target id mismatch");
 			}
 		}
 		break;
@@ -1394,6 +1396,7 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 				if (dm_write(dm_next, wp.seq, DM_PERSIST_IN_FLIGHT_RESET, &mission_item, len) != len) {
 					mavlink_wpm_send_waypoint_ack(_wpm->current_partner_sysid, _wpm->current_partner_compid, MAV_MISSION_ERROR);
+					mavlink_missionlib_send_gcs_string("#audio: Unable to write on micro SD");
 					_wpm->current_state = MAVLINK_WPM_STATE_IDLE;
 					break;
 				}
@@ -1425,9 +1428,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 				} else {
 					mavlink_wpm_send_waypoint_request(_wpm->current_partner_sysid, _wpm->current_partner_compid, _wpm->current_wp_id);
 				}
-
-			} else {
-				mavlink_missionlib_send_gcs_string("REJ. WP CMD: target id mismatch");
 			}
 
 			break;
@@ -1463,11 +1463,6 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 
 					if (_verbose) { warnx("IGN WP CLEAR CMD: Busy"); }
 				}
-
-
-			} else if (wpca.target_system == mavlink_system.sysid /*&& wpca.target_component == mavlink_wpm_comp_id */ && _wpm->current_state != MAVLINK_WPM_STATE_IDLE) {
-
-				mavlink_missionlib_send_gcs_string("REJ. WP CLERR CMD: target id mismatch");
 			}
 
 			break;
@@ -1982,14 +1977,14 @@ 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, _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);
 				}
 
 			} else {
-				warnx("stream %s not found", _subscribe_to_stream, _device_name);
+				warnx("stream %s on device %s not found", _subscribe_to_stream, _device_name);
 			}
 
 			delete _subscribe_to_stream;
@@ -2195,7 +2190,6 @@ Mavlink::stream(int argc, char *argv[])
 	const char *device_name = DEFAULT_DEVICE_NAME;
 	float rate = -1.0f;
 	const char *stream_name = nullptr;
-	int ch;
 
 	argc -= 2;
 	argv += 2;
@@ -2232,7 +2226,7 @@ Mavlink::stream(int argc, char *argv[])
 		i++;
 	}
 
-	if (!err_flag && rate >= 0.0 && stream_name != nullptr) {
+	if (!err_flag && rate >= 0.0f && stream_name != nullptr) {
 		Mavlink *inst = get_instance_for_device(device_name);
 
 		if (inst != nullptr) {
diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h
index 1f0445cb6..85a88442c 100644
--- a/src/modules/mavlink/mavlink_main.h
+++ b/src/modules/mavlink/mavlink_main.h
@@ -93,6 +93,7 @@ struct mavlink_wpm_storage {
 	uint8_t current_partner_compid;
 	uint64_t timestamp_lastaction;
 	uint64_t timestamp_last_send_setpoint;
+	uint64_t timestamp_last_send_request;
 	uint32_t timeout;
 	int current_dataman_id;
 };
@@ -221,8 +222,6 @@ private:
 	int		_mavlink_fd;
 	bool		_task_running;
 
-	perf_counter_t	_loop_perf;			/**< loop performance counter */
-
 	/* states */
 	bool		_hil_enabled;		/**< Hardware In the Loop mode */
 	bool		_use_hil_gps;		/**< Accept GPS HIL messages (for example from an external motion capturing system to fake indoor gps) */
@@ -282,6 +281,7 @@ private:
 
 	pthread_mutex_t		_message_buffer_mutex;
 
+	perf_counter_t	_loop_perf;			/**< loop performance counter */
 	bool			_param_initialized;
 	param_t			_param_system_id;
 	param_t			_param_component_id;
diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 748c1f69a..184726fe8 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -138,6 +138,10 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			*mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO;
 			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
+
+		} else if (status->main_state == MAIN_STATE_ACRO) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
 		}
 
 	} else {
@@ -290,7 +294,7 @@ protected:
 						status.onboard_control_sensors_health,
 						status.load * 1000.0f,
 						status.battery_voltage * 1000.0f,
-						status.battery_current * 1000.0f,
+						status.battery_current * 100.0f,
 						status.battery_remaining * 100.0f,
 						status.drop_rate_comm,
 						status.errors_comm,
@@ -305,7 +309,7 @@ protected:
 class MavlinkStreamHighresIMU : public MavlinkStream
 {
 public:
-	~MavlinkStreamHighresIMU();
+	~MavlinkStreamHighresIMU() {};
 
 	const char *get_name() const
 	{
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 666b3a8cd..9c528adbe 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -932,6 +932,8 @@ void *MavlinkReceiver::start_helper(void *context)
 	rcv->receive_thread(NULL);
 
 	delete rcv;
+
+	return nullptr;
 }
 
 pthread_t
diff --git a/src/modules/mavlink/mavlink_stream.cpp b/src/modules/mavlink/mavlink_stream.cpp
index bb19d7e33..5ec30bd33 100644
--- a/src/modules/mavlink/mavlink_stream.cpp
+++ b/src/modules/mavlink/mavlink_stream.cpp
@@ -81,5 +81,9 @@ MavlinkStream::update(const hrt_abstime t)
 		/* interval expired, send message */
 		send(t);
 		_last_sent = (t / _interval) * _interval;
+
+		return 0;
 	}
+
+	return -1;
 }
diff --git a/src/modules/mavlink/mavlink_stream.h b/src/modules/mavlink/mavlink_stream.h
index eb881edd7..a41ace48e 100644
--- a/src/modules/mavlink/mavlink_stream.h
+++ b/src/modules/mavlink/mavlink_stream.h
@@ -55,9 +55,13 @@ public:
 	MavlinkStream *next;
 
 	MavlinkStream();
-	~MavlinkStream();
+	virtual ~MavlinkStream();
 	void set_interval(const unsigned int interval);
 	void set_channel(mavlink_channel_t channel);
+
+	/**
+	 * @return 0 if updated / sent, -1 if unchanged
+	 */
 	int update(const hrt_abstime t);
 	static MavlinkStream *new_instance();
 	static const char *get_name_static();
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index b23166a5e..20e016da3 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -162,6 +162,9 @@ private:
 		param_t man_roll_max;
 		param_t man_pitch_max;
 		param_t man_yaw_max;
+		param_t acro_roll_max;
+		param_t acro_pitch_max;
+		param_t acro_yaw_max;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -175,6 +178,7 @@ private:
 		float man_roll_max;
 		float man_pitch_max;
 		float man_yaw_max;
+		math::Vector<3> acro_rate_max;		/**< max attitude rates in acro mode */
 	}		_params;
 
 	/**
@@ -284,6 +288,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.man_roll_max = 0.0f;
 	_params.man_pitch_max = 0.0f;
 	_params.man_yaw_max = 0.0f;
+	_params.acro_rate_max.zero();
 
 	_rates_prev.zero();
 	_rates_sp.zero();
@@ -310,6 +315,9 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.man_roll_max	= 	param_find("MC_MAN_R_MAX");
 	_params_handles.man_pitch_max	= 	param_find("MC_MAN_P_MAX");
 	_params_handles.man_yaw_max		= 	param_find("MC_MAN_Y_MAX");
+	_params_handles.acro_roll_max	= 	param_find("MC_ACRO_R_MAX");
+	_params_handles.acro_pitch_max	= 	param_find("MC_ACRO_P_MAX");
+	_params_handles.acro_yaw_max		= 	param_find("MC_ACRO_Y_MAX");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -386,6 +394,14 @@ MulticopterAttitudeControl::parameters_update()
 	_params.man_pitch_max = math::radians(_params.man_pitch_max);
 	_params.man_yaw_max = math::radians(_params.man_yaw_max);
 
+	/* acro control scale */
+	param_get(_params_handles.acro_roll_max, &v);
+	_params.acro_rate_max(0) = math::radians(v);
+	param_get(_params_handles.acro_pitch_max, &v);
+	_params.acro_rate_max(1) = math::radians(v);
+	param_get(_params_handles.acro_yaw_max, &v);
+	_params.acro_rate_max(2) = math::radians(v);
+
 	return OK;
 }
 
@@ -782,11 +798,36 @@ MulticopterAttitudeControl::task_main()
 
 			} else {
 				/* attitude controller disabled, poll rates setpoint topic */
-				vehicle_rates_setpoint_poll();
-				_rates_sp(0) = _v_rates_sp.roll;
-				_rates_sp(1) = _v_rates_sp.pitch;
-				_rates_sp(2) = _v_rates_sp.yaw;
-				_thrust_sp = _v_rates_sp.thrust;
+				if (_v_control_mode.flag_control_manual_enabled) {
+					/* manual rates control - ACRO mode */
+					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
+					_thrust_sp = _manual_control_sp.z;
+
+					/* reset yaw setpoint after ACRO */
+					_reset_yaw_sp = true;
+
+					/* publish attitude rates setpoint */
+					_v_rates_sp.roll = _rates_sp(0);
+					_v_rates_sp.pitch = _rates_sp(1);
+					_v_rates_sp.yaw = _rates_sp(2);
+					_v_rates_sp.thrust = _thrust_sp;
+					_v_rates_sp.timestamp = hrt_absolute_time();
+
+					if (_v_rates_sp_pub > 0) {
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp);
+
+					} else {
+						_v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp);
+					}
+
+				} else {
+					/* attitude controller disabled, poll rates setpoint topic */
+					vehicle_rates_setpoint_poll();
+					_rates_sp(0) = _v_rates_sp.roll;
+					_rates_sp(1) = _v_rates_sp.pitch;
+					_rates_sp(2) = _v_rates_sp.yaw;
+					_thrust_sp = _v_rates_sp.thrust;
+				}
 			}
 
 			if (_v_control_mode.flag_control_rates_enabled) {
diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c
index ad38a0a03..819847b40 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -215,3 +215,32 @@ PARAM_DEFINE_FLOAT(MC_MAN_P_MAX, 35.0f);
  * @group Multicopter Attitude Control
  */
 PARAM_DEFINE_FLOAT(MC_MAN_Y_MAX, 120.0f);
+
+/**
+ * Max acro roll rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_R_MAX, 90.0f);
+
+/**
+ * Max acro pitch rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_P_MAX, 90.0f);
+
+/**
+ * Max acro yaw rate
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_ACRO_Y_MAX, 120.0f);
diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp
index 2f45f2267..e1a6854b2 100644
--- a/src/modules/navigator/mission_feasibility_checker.cpp
+++ b/src/modules/navigator/mission_feasibility_checker.cpp
@@ -63,7 +63,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
 }
 
 
-bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
+bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
 {
 	/* Init if not done yet */
 	init();
@@ -76,24 +76,24 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_
 
 
 	if (isRotarywing)
-		return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence);
+		return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
 	else
-		return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence);
+		return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
 }
 
-bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
+bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
 {
 
-	return checkGeofence(dm_current, nMissionItems, geofence);
+	return  (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
 }
 
-bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
+bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
 {
 	/* Update fixed wing navigation capabilites */
 	updateNavigationCapabilities();
 //	warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement);
 
-	return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence));
+	return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt));
 }
 
 bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence)
@@ -109,7 +109,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 				return false;
 			}
 
-			if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude
+			if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) {
 				mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i);
 				return false;
 			}
@@ -119,6 +119,36 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 	return true;
 }
 
+bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error)
+{
+	/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
+	for (size_t i = 0; i < nMissionItems; i++) {
+		static struct mission_item_s missionitem;
+		const ssize_t len = sizeof(struct mission_item_s);
+
+		if (dm_read(dm_current, i, &missionitem, len) != len) {
+			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
+			if (throw_error) {
+				return false;
+			} else	{
+				return true;
+			}
+		}
+
+		if (home_alt > missionitem.altitude) {
+			if (throw_error) {
+				mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i);
+				return false;
+			} else	{
+				mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i);
+				return true;
+			}
+		}
+	}
+
+	return true;
+}
+
 bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems)
 {
 	/* Go through all mission items and search for a landing waypoint
diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h
index f98e28462..96c9209d3 100644
--- a/src/modules/navigator/mission_feasibility_checker.h
+++ b/src/modules/navigator/mission_feasibility_checker.h
@@ -61,14 +61,15 @@ private:
 
 	/* Checks for all airframes */
 	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
+	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false);
 
 	/* Checks specific to fixedwing airframes */
-	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
+	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
 	bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
 	void updateNavigationCapabilities();
 
 	/* Checks specific to rotarywing airframes */
-	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
+	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
 public:
 
 	MissionFeasibilityChecker();
@@ -77,7 +78,7 @@ public:
 	/*
 	 * Returns true if mission is feasible and false otherwise
 	 */
-	bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
+	bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
 
 };
 
diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp
index 87c893079..06e0c5904 100644
--- a/src/modules/navigator/navigator_main.cpp
+++ b/src/modules/navigator/navigator_main.cpp
@@ -519,7 +519,7 @@ Navigator::offboard_mission_update(bool isrotaryWing)
 			dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1;
 		}
 
-		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence);
+		missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence, _home_pos.alt);
 
 		_mission.set_offboard_dataman_id(offboard_mission.dataman_id);
 
@@ -1458,7 +1458,6 @@ Navigator::check_mission_item_reached()
 
 	/* XXX TODO count turns */
 	if ((_mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT ||
-	     _mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT ||
 	     _mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED) &&
 	    _mission_item.loiter_radius > 0.01f) {
 
@@ -1477,27 +1476,27 @@ Navigator::check_mission_item_reached()
 			acceptance_radius = _parameters.acceptance_radius;
 		}
 
-		float dist = -1.0f;
-		float dist_xy = -1.0f;
-		float dist_z = -1.0f;
-
-		/* calculate AMSL altitude for this waypoint */
-		float wp_alt_amsl = _mission_item.altitude;
-
-		if (_mission_item.altitude_is_relative)
-			wp_alt_amsl += _home_pos.alt;
-
-		dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
-				(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
-				&dist_xy, &dist_z);
-
 		if (_do_takeoff) {
-			if (_global_pos.alt > wp_alt_amsl - acceptance_radius) {
-				/* require only altitude for takeoff */
+			/* require only altitude for takeoff */
+			if (_global_pos.alt > _pos_sp_triplet.current.alt - acceptance_radius) {
 				_waypoint_position_reached = true;
 			}
 
 		} else {
+			float dist = -1.0f;
+			float dist_xy = -1.0f;
+			float dist_z = -1.0f;
+
+			/* calculate AMSL altitude for this waypoint */
+			float wp_alt_amsl = _mission_item.altitude;
+
+			if (_mission_item.altitude_is_relative)
+				wp_alt_amsl += _home_pos.alt;
+
+			dist = get_distance_to_point_global_wgs84(_mission_item.lat, _mission_item.lon, wp_alt_amsl,
+					(double)_global_pos.lat, (double)_global_pos.lon, _global_pos.alt,
+					&dist_xy, &dist_z);
+
 			if (dist >= 0.0f && dist <= acceptance_radius) {
 				_waypoint_position_reached = true;
 			}
@@ -1567,7 +1566,14 @@ Navigator::on_mission_item_reached()
 		}
 
 		if (_mission.current_mission_available()) {
-			set_mission_item();
+			if (_mission_item.autocontinue) {
+				/* continue mission */
+				set_mission_item();
+
+			} else {
+				/* autocontinue disabled for this item */
+				request_loiter_or_ready();
+			}
 
 		} else {
 			/* if no more mission items available then finish mission */
diff --git a/src/modules/position_estimator_inav/inertial_filter.c b/src/modules/position_estimator_inav/inertial_filter.c
index 2f1b3c014..a36a4688d 100644
--- a/src/modules/position_estimator_inav/inertial_filter.c
+++ b/src/modules/position_estimator_inav/inertial_filter.c
@@ -9,15 +9,18 @@
 
 #include "inertial_filter.h"
 
-void inertial_filter_predict(float dt, float x[3])
+void inertial_filter_predict(float dt, float x[2], float acc)
 {
 	if (isfinite(dt)) {
-		x[0] += x[1] * dt + x[2] * dt * dt / 2.0f;
-		x[1] += x[2] * dt;
+		if (!isfinite(acc)) {
+			acc = 0.0f;
+		}
+		x[0] += x[1] * dt + acc * dt * dt / 2.0f;
+		x[1] += acc * dt;
 	}
 }
 
-void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
+void inertial_filter_correct(float e, float dt, float x[2], int i, float w)
 {
 	if (isfinite(e) && isfinite(w) && isfinite(dt)) {
 		float ewdt = e * w * dt;
@@ -25,10 +28,6 @@ void inertial_filter_correct(float e, float dt, float x[3], int i, float w)
 
 		if (i == 0) {
 			x[1] += w * ewdt;
-			x[2] += w * w * ewdt / 3.0;
-
-		} else if (i == 1) {
-			x[2] += w * ewdt;
 		}
 	}
 }
diff --git a/src/modules/position_estimator_inav/inertial_filter.h b/src/modules/position_estimator_inav/inertial_filter.h
index 761c17097..cdeb4cfc6 100644
--- a/src/modules/position_estimator_inav/inertial_filter.h
+++ b/src/modules/position_estimator_inav/inertial_filter.h
@@ -8,6 +8,6 @@
 #include <stdbool.h>
 #include <drivers/drv_hrt.h>
 
-void inertial_filter_predict(float dt, float x[3]);
+void inertial_filter_predict(float dt, float x[3], float acc);
 
 void inertial_filter_correct(float e, float dt, float x[3], int i, float w);
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index d8d0ff37d..f908d7a3b 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -71,19 +71,20 @@
 #include "inertial_filter.h"
 
 #define MIN_VALID_W 0.00001f
+#define PUB_INTERVAL 10000	// limit publish rate to 100 Hz
+#define EST_BUF_SIZE 250000 / PUB_INTERVAL		// buffer size is 0.5s
 
 static bool thread_should_exit = false; /**< Deamon exit flag */
 static bool thread_running = false; /**< Deamon status flag */
 static int position_estimator_inav_task; /**< Handle of deamon task / thread */
 static bool verbose_mode = false;
 
-static const hrt_abstime gps_topic_timeout = 1000000;		// GPS topic timeout = 1s
+static const hrt_abstime gps_topic_timeout = 500000;		// GPS topic timeout = 0.5s
 static const hrt_abstime flow_topic_timeout = 1000000;	// optical flow topic timeout = 1s
 static const hrt_abstime sonar_timeout = 150000;	// sonar timeout = 150ms
 static const hrt_abstime sonar_valid_timeout = 1000000;	// estimate sonar distance during this time after sonar loss
 static const hrt_abstime xy_src_timeout = 2000000;	// estimate position during this time after position sources loss
 static const uint32_t updates_counter_len = 1000000;
-static const uint32_t pub_interval = 10000;	// limit publish rate to 100 Hz
 static const float max_flow = 1.0f;	// max flow value that can be used, rad/s
 
 __EXPORT int position_estimator_inav_main(int argc, char *argv[]);
@@ -92,6 +93,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[]);
 
 static void usage(const char *reason);
 
+static inline int min(int val1, int val2)
+{
+	return (val1 < val2) ? val1 : val2;
+}
+
+static inline int max(int val1, int val2)
+{
+	return (val1 > val2) ? val1 : val2;
+}
+
 /**
  * Print the correct usage.
  */
@@ -135,7 +146,7 @@ int position_estimator_inav_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		position_estimator_inav_task = task_spawn_cmd("position_estimator_inav",
-					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000,
+					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 5000,
 					       position_estimator_inav_thread_main,
 					       (argv) ? (const char **) &argv[2] : (const char **) NULL);
 		exit(0);
@@ -168,15 +179,15 @@ int position_estimator_inav_main(int argc, char *argv[])
 	exit(1);
 }
 
-void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3], float z_est[3], float x_est_prev[3], float y_est_prev[3], float z_est_prev[3], float corr_acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
+void write_debug_log(const char *msg, float dt, float x_est[2], float y_est[2], float z_est[2], float x_est_prev[2], float y_est_prev[2], float z_est_prev[2], float acc[3], float corr_gps[3][2], float w_xy_gps_p, float w_xy_gps_v)
 {
 	FILE *f = fopen("/fs/microsd/inav.log", "a");
 
 	if (f) {
 		char *s = malloc(256);
-		unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f] x_est_prev=[%.5f %.5f %.5f] y_est_prev=[%.5f %.5f %.5f] z_est_prev=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2], x_est_prev[0], x_est_prev[1], x_est_prev[2], y_est_prev[0], y_est_prev[1], y_est_prev[2], z_est_prev[0], z_est_prev[1], z_est_prev[2]);
+		unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f] y_est=[%.5f %.5f] z_est=[%.5f %.5f] x_est_prev=[%.5f %.5f] y_est_prev=[%.5f %.5f] z_est_prev=[%.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], y_est[0], y_est[1], z_est[0], z_est[1], x_est_prev[0], x_est_prev[1], y_est_prev[0], y_est_prev[1], z_est_prev[0], z_est_prev[1]);
 		fwrite(s, 1, n, f);
-		n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
+		n = snprintf(s, 256, "\tacc=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", acc[0], acc[1], acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
 		fwrite(s, 1, n, f);
 		free(s);
 	}
@@ -195,11 +206,27 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 	mavlink_log_info(mavlink_fd, "[inav] started");
 
-	float x_est[3] = { 0.0f, 0.0f, 0.0f };
-	float y_est[3] = { 0.0f, 0.0f, 0.0f };
-	float z_est[3] = { 0.0f, 0.0f, 0.0f };
+	float x_est[2] = { 0.0f, 0.0f };	// pos, vel
+	float y_est[2] = { 0.0f, 0.0f };	// pos, vel
+	float z_est[2] = { 0.0f, 0.0f };	// pos, vel
 
-	float x_est_prev[3], y_est_prev[3], z_est_prev[3];
+	float est_buf[EST_BUF_SIZE][3][2];	// estimated position buffer
+	float R_buf[EST_BUF_SIZE][3][3];	// rotation matrix buffer
+	float R_gps[3][3];					// rotation matrix for GPS correction moment
+	memset(est_buf, 0, sizeof(est_buf));
+	memset(R_buf, 0, sizeof(R_buf));
+	memset(R_gps, 0, sizeof(R_gps));
+	int buf_ptr = 0;
+
+	static const float min_eph_epv = 2.0f;	// min EPH/EPV, used for weight calculation
+	static const float max_eph_epv = 20.0f;	// max EPH/EPV acceptable for estimation
+
+	float eph = max_eph_epv;
+	float epv = 1.0f;
+
+	float eph_flow = 1.0f;
+
+	float x_est_prev[2], y_est_prev[2], z_est_prev[2];
 	memset(x_est_prev, 0, sizeof(x_est_prev));
 	memset(y_est_prev, 0, sizeof(y_est_prev));
 	memset(z_est_prev, 0, sizeof(z_est_prev));
@@ -238,7 +265,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float accel_NED[3] = { 0.0f, 0.0f, -CONSTANTS_ONE_G };
 
 	/* store error when sensor updates, but correct on each time step to avoid jumps in estimated value */
-	float corr_acc[] = { 0.0f, 0.0f, 0.0f };	// N E D
+	float acc[] = { 0.0f, 0.0f, 0.0f };	// N E D
 	float acc_bias[] = { 0.0f, 0.0f, 0.0f };	// body frame
 	float corr_baro = 0.0f;		// D
 	float corr_gps[3][2] = {
@@ -254,9 +281,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float corr_flow[] = { 0.0f, 0.0f };	// N E
 	float w_flow = 0.0f;
 
-	static float min_eph_epv = 2.0f;	// min EPH/EPV, used for weight calculation
-	static float max_eph_epv = 10.0f;	// max EPH/EPV acceptable for estimation
-
 	float sonar_prev = 0.0f;
 	hrt_abstime flow_prev = 0;			// time of last flow measurement
 	hrt_abstime sonar_time = 0;			// time of last sonar measurement (not filtered)
@@ -338,8 +362,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					/* mean calculation over several measurements */
 					if (baro_init_cnt < baro_init_num) {
-						baro_offset += sensor.baro_alt_meter;
-						baro_init_cnt++;
+						if (isfinite(sensor.baro_alt_meter)) {
+							baro_offset += sensor.baro_alt_meter;
+							baro_init_cnt++;
+						}
 
 					} else {
 						wait_baro = false;
@@ -415,19 +441,17 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 						/* transform acceleration vector from body frame to NED frame */
 						for (int i = 0; i < 3; i++) {
-							accel_NED[i] = 0.0f;
+							acc[i] = 0.0f;
 
 							for (int j = 0; j < 3; j++) {
-								accel_NED[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
+								acc[i] += att.R[i][j] * sensor.accelerometer_m_s2[j];
 							}
 						}
 
-						corr_acc[0] = accel_NED[0] - x_est[2];
-						corr_acc[1] = accel_NED[1] - y_est[2];
-						corr_acc[2] = accel_NED[2] + CONSTANTS_ONE_G - z_est[2];
+						acc[2] += CONSTANTS_ONE_G;
 
 					} else {
-						memset(corr_acc, 0, sizeof(corr_acc));
+						memset(acc, 0, sizeof(acc));
 					}
 
 					accel_timestamp = sensor.accelerometer_timestamp;
@@ -533,6 +557,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						w_flow *= 0.05f;
 					}
 
+					/* under ideal conditions, on 1m distance assume EPH = 10cm */
+					eph_flow = 0.1 / w_flow;
+
 					flow_valid = true;
 
 				} else {
@@ -592,13 +619,13 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				/* hysteresis for GPS quality */
 				if (gps_valid) {
-					if (gps.eph_m > max_eph_epv * 1.5f || gps.epv_m > max_eph_epv * 1.5f || gps.fix_type < 3) {
+					if (gps.eph_m > max_eph_epv || gps.epv_m > max_eph_epv || gps.fix_type < 3) {
 						gps_valid = false;
 						mavlink_log_info(mavlink_fd, "[inav] GPS signal lost");
 					}
 
 				} else {
-					if (gps.eph_m < max_eph_epv && gps.epv_m < max_eph_epv && gps.fix_type >= 3) {
+					if (gps.eph_m < max_eph_epv * 0.7f && gps.epv_m < max_eph_epv * 0.7f && gps.fix_type >= 3) {
 						gps_valid = true;
 						reset_est = true;
 						mavlink_log_info(mavlink_fd, "[inav] GPS signal found");
@@ -623,11 +650,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							/* set position estimate to (0, 0, 0), use GPS velocity for XY */
 							x_est[0] = 0.0f;
 							x_est[1] = gps.vel_n_m_s;
-							x_est[2] = accel_NED[0];
 							y_est[0] = 0.0f;
 							y_est[1] = gps.vel_e_m_s;
 							z_est[0] = 0.0f;
-							y_est[2] = accel_NED[1];
 
 							local_pos.ref_lat = lat;
 							local_pos.ref_lon = lon;
@@ -650,22 +675,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						if (reset_est) {
 							x_est[0] = gps_proj[0];
 							x_est[1] = gps.vel_n_m_s;
-							x_est[2] = accel_NED[0];
 							y_est[0] = gps_proj[1];
 							y_est[1] = gps.vel_e_m_s;
-							y_est[2] = accel_NED[1];
+						}
+
+						/* calculate index of estimated values in buffer */
+						int est_i = buf_ptr - 1 - min(EST_BUF_SIZE - 1, max(0, (int)(params.delay_gps * 1000000.0f / PUB_INTERVAL)));
+						if (est_i < 0) {
+							est_i += EST_BUF_SIZE;
 						}
 
 						/* calculate correction for position */
-						corr_gps[0][0] = gps_proj[0] - x_est[0];
-						corr_gps[1][0] = gps_proj[1] - y_est[0];
-						corr_gps[2][0] = local_pos.ref_alt - alt - z_est[0];
+						corr_gps[0][0] = gps_proj[0] - est_buf[est_i][0][0];
+						corr_gps[1][0] = gps_proj[1] - est_buf[est_i][1][0];
+						corr_gps[2][0] = local_pos.ref_alt - alt - est_buf[est_i][2][0];
 
 						/* calculate correction for velocity */
 						if (gps.vel_ned_valid) {
-							corr_gps[0][1] = gps.vel_n_m_s - x_est[1];
-							corr_gps[1][1] = gps.vel_e_m_s - y_est[1];
-							corr_gps[2][1] = gps.vel_d_m_s - z_est[1];
+							corr_gps[0][1] = gps.vel_n_m_s - est_buf[est_i][0][1];
+							corr_gps[1][1] = gps.vel_e_m_s - est_buf[est_i][1][1];
+							corr_gps[2][1] = gps.vel_d_m_s - est_buf[est_i][2][1];
 
 						} else {
 							corr_gps[0][1] = 0.0f;
@@ -673,6 +702,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							corr_gps[2][1] = 0.0f;
 						}
 
+						/* save rotation matrix at this moment */
+						memcpy(R_gps, R_buf[est_i], sizeof(R_gps));
+
 						w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
 						w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
 					}
@@ -712,6 +744,14 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		dt = fmaxf(fminf(0.02, dt), 0.002);		// constrain dt from 2 to 20 ms
 		t_prev = t;
 
+		/* increase EPH/EPV on each step */
+		if (eph < max_eph_epv) {
+			eph *= 1.0 + dt;
+		}
+		if (epv < max_eph_epv) {
+			epv += 0.005 * dt;	// add 1m to EPV each 200s (baro drift)
+		}
+
 		/* use GPS if it's valid and reference position initialized */
 		bool use_gps_xy = ref_inited && gps_valid && params.w_xy_gps_p > MIN_VALID_W;
 		bool use_gps_z = ref_inited && gps_valid && params.w_z_gps_p > MIN_VALID_W;
@@ -723,7 +763,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			xy_src_time = t;
 		}
 
-		bool can_estimate_xy = (t < xy_src_time + xy_src_timeout);
+		bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow;
 
 		bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);
 
@@ -755,7 +795,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			corr_baro += offs_corr;
 		}
 
-		/* accelerometer bias correction */
+		/* accelerometer bias correction for GPS (use buffered rotation matrix) */
 		float accel_bias_corr[3] = { 0.0f, 0.0f, 0.0f };
 
 		if (use_gps_xy) {
@@ -769,6 +809,24 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			accel_bias_corr[2] -= corr_gps[2][0] * w_z_gps_p * w_z_gps_p;
 		}
 
+		/* transform error vector from NED frame to body frame */
+		for (int i = 0; i < 3; i++) {
+			float c = 0.0f;
+
+			for (int j = 0; j < 3; j++) {
+				c += R_gps[j][i] * accel_bias_corr[j];
+			}
+
+			if (isfinite(c)) {
+				acc_bias[i] += c * params.w_acc_bias * dt;
+			}
+		}
+
+		/* accelerometer bias correction for flow and baro (assume that there is no delay) */
+		accel_bias_corr[0] = 0.0f;
+		accel_bias_corr[1] = 0.0f;
+		accel_bias_corr[2] = 0.0f;
+
 		if (use_flow) {
 			accel_bias_corr[0] -= corr_flow[0] * params.w_xy_flow;
 			accel_bias_corr[1] -= corr_flow[1] * params.w_xy_flow;
@@ -784,26 +842,31 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				c += att.R[j][i] * accel_bias_corr[j];
 			}
 
-			acc_bias[i] += c * params.w_acc_bias * dt;
+			if (isfinite(c)) {
+				acc_bias[i] += c * params.w_acc_bias * dt;
+			}
 		}
 
 		/* inertial filter prediction for altitude */
-		inertial_filter_predict(dt, z_est);
+		inertial_filter_predict(dt, z_est, acc[2]);
 
-		if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) {
-			write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+		if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
+			write_debug_log("BAD ESTIMATE AFTER Z PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
 			memcpy(z_est, z_est_prev, sizeof(z_est));
 		}
 
 		/* inertial filter correction for altitude */
 		inertial_filter_correct(corr_baro, dt, z_est, 0, params.w_z_baro);
-		inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
-		inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
 
-		if (!(isfinite(z_est[0]) && isfinite(z_est[1]) && isfinite(z_est[2]))) {
-			write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+		if (use_gps_z) {
+			epv = fminf(epv, gps.epv_m);
+
+			inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
+		}
+
+		if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
+			write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
 			memcpy(z_est, z_est_prev, sizeof(z_est));
-			memset(corr_acc, 0, sizeof(corr_acc));
 			memset(corr_gps, 0, sizeof(corr_gps));
 			corr_baro = 0;
 
@@ -813,25 +876,26 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 		if (can_estimate_xy) {
 			/* inertial filter prediction for position */
-			inertial_filter_predict(dt, x_est);
-			inertial_filter_predict(dt, y_est);
+			inertial_filter_predict(dt, x_est, acc[0]);
+			inertial_filter_predict(dt, y_est, acc[1]);
 
-			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) {
-				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
+				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
 				memcpy(x_est, x_est_prev, sizeof(x_est));
 				memcpy(y_est, y_est_prev, sizeof(y_est));
 			}
 
 			/* inertial filter correction for position */
-			inertial_filter_correct(corr_acc[0], dt, x_est, 2, params.w_xy_acc);
-			inertial_filter_correct(corr_acc[1], dt, y_est, 2, params.w_xy_acc);
-
 			if (use_flow) {
+				eph = fminf(eph, eph_flow);
+
 				inertial_filter_correct(corr_flow[0], dt, x_est, 1, params.w_xy_flow * w_flow);
 				inertial_filter_correct(corr_flow[1], dt, y_est, 1, params.w_xy_flow * w_flow);
 			}
 
 			if (use_gps_xy) {
+				eph = fminf(eph, gps.eph_m);
+
 				inertial_filter_correct(corr_gps[0][0], dt, x_est, 0, w_xy_gps_p);
 				inertial_filter_correct(corr_gps[1][0], dt, y_est, 0, w_xy_gps_p);
 
@@ -841,11 +905,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				}
 			}
 
-			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(x_est[2]) && isfinite(y_est[0]) && isfinite(y_est[1]) && isfinite(y_est[2]))) {
-				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
+			if (!(isfinite(x_est[0]) && isfinite(x_est[1]) && isfinite(y_est[0]) && isfinite(y_est[1]))) {
+				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev, acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
 				memcpy(x_est, x_est_prev, sizeof(x_est));
 				memcpy(y_est, y_est_prev, sizeof(y_est));
-				memset(corr_acc, 0, sizeof(corr_acc));
 				memset(corr_gps, 0, sizeof(corr_gps));
 				memset(corr_flow, 0, sizeof(corr_flow));
 
@@ -906,8 +969,25 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			}
 		}
 
-		if (t > pub_last + pub_interval) {
+		if (t > pub_last + PUB_INTERVAL) {
 			pub_last = t;
+
+			/* push current estimate to buffer */
+			est_buf[buf_ptr][0][0] = x_est[0];
+			est_buf[buf_ptr][0][1] = x_est[1];
+			est_buf[buf_ptr][1][0] = y_est[0];
+			est_buf[buf_ptr][1][1] = y_est[1];
+			est_buf[buf_ptr][2][0] = z_est[0];
+			est_buf[buf_ptr][2][1] = z_est[1];
+
+			/* push current rotation matrix to buffer */
+			memcpy(R_buf[buf_ptr], att.R, sizeof(att.R));
+
+			buf_ptr++;
+			if (buf_ptr >= EST_BUF_SIZE) {
+				buf_ptr = 0;
+			}
+
 			/* publish local position */
 			local_pos.xy_valid = can_estimate_xy;
 			local_pos.v_xy_valid = can_estimate_xy;
@@ -922,6 +1002,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			local_pos.landed = landed;
 			local_pos.yaw = att.yaw;
 			local_pos.dist_bottom_valid = dist_bottom_valid;
+			local_pos.eph = eph;
+			local_pos.epv = epv;
 
 			if (local_pos.dist_bottom_valid) {
 				local_pos.dist_bottom = -z_est[0] - surface_offset;
@@ -950,9 +1032,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 				global_pos.yaw = local_pos.yaw;
 
-				// TODO implement dead-reckoning
-				global_pos.eph = gps.eph_m;
-				global_pos.epv = gps.epv_m;
+				global_pos.eph = eph;
+				global_pos.epv = epv;
 
 				if (vehicle_global_position_pub < 0) {
 					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
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 2e4f26661..d88419c25 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.c
@@ -42,11 +42,9 @@
 
 PARAM_DEFINE_FLOAT(INAV_W_Z_BARO, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_W_Z_GPS_P, 0.005f);
-PARAM_DEFINE_FLOAT(INAV_W_Z_ACC, 20.0f);
 PARAM_DEFINE_FLOAT(INAV_W_Z_SONAR, 3.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_P, 1.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_GPS_V, 2.0f);
-PARAM_DEFINE_FLOAT(INAV_W_XY_ACC, 20.0f);
 PARAM_DEFINE_FLOAT(INAV_W_XY_FLOW, 5.0f);
 PARAM_DEFINE_FLOAT(INAV_W_GPS_FLOW, 0.1f);
 PARAM_DEFINE_FLOAT(INAV_W_ACC_BIAS, 0.05f);
@@ -57,16 +55,15 @@ PARAM_DEFINE_FLOAT(INAV_SONAR_ERR, 0.5f);
 PARAM_DEFINE_FLOAT(INAV_LAND_T, 3.0f);
 PARAM_DEFINE_FLOAT(INAV_LAND_DISP, 0.7f);
 PARAM_DEFINE_FLOAT(INAV_LAND_THR, 0.2f);
+PARAM_DEFINE_FLOAT(INAV_DELAY_GPS, 0.2f);
 
 int parameters_init(struct position_estimator_inav_param_handles *h)
 {
 	h->w_z_baro = param_find("INAV_W_Z_BARO");
 	h->w_z_gps_p = param_find("INAV_W_Z_GPS_P");
-	h->w_z_acc = param_find("INAV_W_Z_ACC");
 	h->w_z_sonar = param_find("INAV_W_Z_SONAR");
 	h->w_xy_gps_p = param_find("INAV_W_XY_GPS_P");
 	h->w_xy_gps_v = param_find("INAV_W_XY_GPS_V");
-	h->w_xy_acc = param_find("INAV_W_XY_ACC");
 	h->w_xy_flow = param_find("INAV_W_XY_FLOW");
 	h->w_gps_flow = param_find("INAV_W_GPS_FLOW");
 	h->w_acc_bias = param_find("INAV_W_ACC_BIAS");
@@ -77,6 +74,7 @@ int parameters_init(struct position_estimator_inav_param_handles *h)
 	h->land_t = param_find("INAV_LAND_T");
 	h->land_disp = param_find("INAV_LAND_DISP");
 	h->land_thr = param_find("INAV_LAND_THR");
+	h->delay_gps = param_find("INAV_DELAY_GPS");
 
 	return OK;
 }
@@ -85,11 +83,9 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 {
 	param_get(h->w_z_baro, &(p->w_z_baro));
 	param_get(h->w_z_gps_p, &(p->w_z_gps_p));
-	param_get(h->w_z_acc, &(p->w_z_acc));
 	param_get(h->w_z_sonar, &(p->w_z_sonar));
 	param_get(h->w_xy_gps_p, &(p->w_xy_gps_p));
 	param_get(h->w_xy_gps_v, &(p->w_xy_gps_v));
-	param_get(h->w_xy_acc, &(p->w_xy_acc));
 	param_get(h->w_xy_flow, &(p->w_xy_flow));
 	param_get(h->w_gps_flow, &(p->w_gps_flow));
 	param_get(h->w_acc_bias, &(p->w_acc_bias));
@@ -100,6 +96,7 @@ int parameters_update(const struct position_estimator_inav_param_handles *h, str
 	param_get(h->land_t, &(p->land_t));
 	param_get(h->land_disp, &(p->land_disp));
 	param_get(h->land_thr, &(p->land_thr));
+	param_get(h->delay_gps, &(p->delay_gps));
 
 	return OK;
 }
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_params.h b/src/modules/position_estimator_inav/position_estimator_inav_params.h
index e2be079d3..df1cfaa71 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_params.h
+++ b/src/modules/position_estimator_inav/position_estimator_inav_params.h
@@ -43,11 +43,9 @@
 struct position_estimator_inav_params {
 	float w_z_baro;
 	float w_z_gps_p;
-	float w_z_acc;
 	float w_z_sonar;
 	float w_xy_gps_p;
 	float w_xy_gps_v;
-	float w_xy_acc;
 	float w_xy_flow;
 	float w_gps_flow;
 	float w_acc_bias;
@@ -58,16 +56,15 @@ struct position_estimator_inav_params {
 	float land_t;
 	float land_disp;
 	float land_thr;
+	float delay_gps;
 };
 
 struct position_estimator_inav_param_handles {
 	param_t w_z_baro;
 	param_t w_z_gps_p;
-	param_t w_z_acc;
 	param_t w_z_sonar;
 	param_t w_xy_gps_p;
 	param_t w_xy_gps_v;
-	param_t w_xy_acc;
 	param_t w_xy_flow;
 	param_t w_gps_flow;
 	param_t w_acc_bias;
@@ -78,6 +75,7 @@ struct position_estimator_inav_param_handles {
 	param_t land_t;
 	param_t land_disp;
 	param_t land_thr;
+	param_t delay_gps;
 };
 
 /**
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index ebf4f3e8e..2f721bf1e 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -213,6 +213,7 @@ mixer_tick(void)
 		mixed = mixer_group.mix(&outputs[0], PX4IO_SERVO_COUNT);
 		in_mixer = false;
 
+		/* the pwm limit call takes care of out of band errors */
 		pwm_limit_calc(should_arm, mixed, r_page_servo_disarmed, r_page_servo_control_min, r_page_servo_control_max, outputs, r_page_servos, &pwm_limit);
 
 		for (unsigned i = mixed; i < PX4IO_SERVO_COUNT; i++)
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 39f433eb5..577cadfbb 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -89,6 +89,7 @@
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
 #include <version/version.h>
 
 #include <mavlink/mavlink_log.h>
@@ -97,6 +98,36 @@
 #include "sdlog2_format.h"
 #include "sdlog2_messages.h"
 
+/**
+ * Logging rate.
+ *
+ * A value of -1 indicates the commandline argument
+ * should be obeyed. A value of 0 sets the minimum rate,
+ * any other value is interpreted as rate in Hertz. This
+ * parameter is only read out before logging starts (which
+ * commonly is before arming).
+ *
+ * @min -1
+ * @max  1
+ * @group SD Logging
+ */
+PARAM_DEFINE_INT32(SDLOG_RATE, -1);
+
+/**
+ * Enable extended logging mode.
+ *
+ * A value of -1 indicates the commandline argument
+ * should be obeyed. A value of 0 disables extended
+ * logging mode, a value of 1 enables it. This
+ * parameter is only read out before logging starts
+ * (which commonly is before arming).
+ *
+ * @min -1
+ * @max  1
+ * @group SD Logging
+ */
+PARAM_DEFINE_INT32(SDLOG_EXT, -1);
+
 #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \
 		log_msgs_written++; \
 	} else { \
@@ -112,12 +143,14 @@ static bool main_thread_should_exit = false;		/**< Deamon exit flag */
 static bool thread_running = false;			/**< Deamon status flag */
 static int deamon_task;						/**< Handle of deamon task / thread */
 static bool logwriter_should_exit = false;	/**< Logwriter thread exit flag */
-static const int MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
-static const int MAX_NO_LOGFILE = 999;		/**< Maximum number of log files */
+static const unsigned MAX_NO_LOGFOLDER = 999;	/**< Maximum number of log dirs */
+static const unsigned MAX_NO_LOGFILE = 999;		/**< Maximum number of log files */
 static const int LOG_BUFFER_SIZE_DEFAULT = 8192;
 static const int MAX_WRITE_CHUNK = 512;
 static const int MIN_BYTES_TO_WRITE = 512;
 
+static bool _extended_logging = false;
+
 static const char *log_root = "/fs/microsd/log";
 static int mavlink_fd = -1;
 struct logbuffer_s lb;
@@ -218,6 +251,8 @@ static int create_log_dir(void);
  */
 static int open_log_file(void);
 
+static int open_perf_file(const char* str);
+
 static void
 sdlog2_usage(const char *reason)
 {
@@ -225,12 +260,13 @@ sdlog2_usage(const char *reason)
 		fprintf(stderr, "%s\n", reason);
 	}
 
-	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t\n"
+	errx(1, "usage: sdlog2 {start|stop|status} [-r <log rate>] [-b <buffer size>] -e -a -t -x\n"
 		 "\t-r\tLog rate in Hz, 0 means unlimited rate\n"
 		 "\t-b\tLog buffer size in KiB, default is 8\n"
 		 "\t-e\tEnable logging by default (if not, can be started by command)\n"
 		 "\t-a\tLog only when armed (can be still overriden by command)\n"
-		 "\t-t\tUse date/time for naming log directories and files\n");
+		 "\t-t\tUse date/time for naming log directories and files\n"
+		 "\t-x\tExtended logging");
 }
 
 /**
@@ -349,8 +385,8 @@ int create_log_dir()
 int open_log_file()
 {
 	/* string to hold the path to the log */
-	char log_file_name[16] = "";
-	char log_file_path[48] = "";
+	char log_file_name[32] = "";
+	char log_file_path[64] = "";
 
 	if (log_name_timestamp && gps_time != 0) {
 		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
@@ -378,7 +414,7 @@ int open_log_file()
 
 		if (file_number > MAX_NO_LOGFILE) {
 			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
-			warnx("all %d possible files exist already", MAX_NO_LOGFILE);
+			mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
 			return -1;
 		}
 	}
@@ -387,7 +423,58 @@ int open_log_file()
 
 	if (fd < 0) {
 		warn("failed opening log: %s", log_file_name);
-		mavlink_log_info(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+		mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
+
+	} else {
+		warnx("log file: %s", log_file_name);
+		mavlink_log_info(mavlink_fd, "[sdlog2] log file: %s", log_file_name);
+	}
+
+	return fd;
+}
+
+int open_perf_file(const char* str)
+{
+	/* string to hold the path to the log */
+	char log_file_name[32] = "";
+	char log_file_path[64] = "";
+
+	if (log_name_timestamp && gps_time != 0) {
+		/* use GPS time for log file naming, e.g. /fs/microsd/2014-01-19/19_37_52.bin */
+		time_t gps_time_sec = gps_time / 1000000;
+		struct tm t;
+		gmtime_r(&gps_time_sec, &t);
+		strftime(log_file_name, sizeof(log_file_name), "perf%H_%M_%S.txt", &t);
+		snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
+
+	} else {
+		unsigned file_number = 1; // start with file log001
+
+		/* look for the next file that does not exist */
+		while (file_number <= MAX_NO_LOGFILE) {
+			/* format log file path: e.g. /fs/microsd/sess001/log001.bin */
+			snprintf(log_file_name, sizeof(log_file_name), "perf%03u.txt", file_number);
+			snprintf(log_file_path, sizeof(log_file_path), "%s/%s_%s", log_dir, str, log_file_name);
+
+			if (!file_exist(log_file_path)) {
+				break;
+			}
+
+			file_number++;
+		}
+
+		if (file_number > MAX_NO_LOGFILE) {
+			/* we should not end up here, either we have more than MAX_NO_LOGFILE on the SD card, or another problem */
+			mavlink_log_critical(mavlink_fd, "[sdlog2] ERR: max files %d", MAX_NO_LOGFILE);
+			return -1;
+		}
+	}
+
+	int fd = open(log_file_path, O_CREAT | O_WRONLY | O_DSYNC);
+
+	if (fd < 0) {
+		warn("failed opening log: %s", log_file_name);
+		mavlink_log_critical(mavlink_fd, "[sdlog2] failed opening log: %s", log_file_name);
 
 	} else {
 		warnx("log file: %s", log_file_name);
@@ -529,6 +616,12 @@ void sdlog2_start_log()
 		errx(1, "error creating logwriter thread");
 	}
 
+	/* write all performance counters */
+	int perf_fd = open_perf_file("preflight");
+	dprintf(perf_fd, "PERFORMANCE COUNTERS PRE-FLIGHT\n\n");
+	perf_print_all(perf_fd);
+	close(perf_fd);
+
 	logging_enabled = true;
 }
 
@@ -556,6 +649,12 @@ void sdlog2_stop_log()
 	logwriter_pthread = 0;
 	pthread_attr_destroy(&logwriter_attr);
 
+	/* write all performance counters */
+	int perf_fd = open_perf_file("postflight");
+	dprintf(perf_fd, "PERFORMANCE COUNTERS POST-FLIGHT\n\n");
+	perf_print_all(perf_fd);
+	close(perf_fd);
+
 	sdlog2_status();
 }
 
@@ -572,7 +671,7 @@ int write_formats(int fd)
 	int written = 0;
 
 	/* fill message format packet for each format and write it */
-	for (int i = 0; i < log_formats_num; i++) {
+	for (unsigned i = 0; i < log_formats_num; i++) {
 		log_msg_format.body = log_formats[i];
 		written += write(fd, &log_msg_format, sizeof(log_msg_format));
 	}
@@ -679,7 +778,7 @@ int sdlog2_thread_main(int argc, char *argv[])
 	 * set error flag instead */
 	bool err_flag = false;
 
-	while ((ch = getopt(argc, argv, "r:b:eat")) != EOF) {
+	while ((ch = getopt(argc, argv, "r:b:eatx")) != EOF) {
 		switch (ch) {
 		case 'r': {
 				unsigned long r = strtoul(optarg, NULL, 10);
@@ -715,6 +814,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_name_timestamp = true;
 			break;
 
+		case 'x':
+			_extended_logging = true;
+			break;
+
 		case '?':
 			if (optopt == 'c') {
 				warnx("option -%c requires an argument", optopt);
@@ -741,6 +844,44 @@ int sdlog2_thread_main(int argc, char *argv[])
 
 	gps_time = 0;
 
+	/* interpret logging params */
+
+	param_t log_rate_ph = param_find("SDLOG_RATE");
+
+	if (log_rate_ph != PARAM_INVALID) {
+		int32_t param_log_rate;
+		param_get(log_rate_ph, &param_log_rate);
+
+		if (param_log_rate > 0) {
+
+			/* we can't do more than ~ 500 Hz, even with a massive buffer */
+			if (param_log_rate > 500) {
+				param_log_rate = 500;
+			}
+
+			sleep_delay = 1000000 / param_log_rate;
+		} else if (param_log_rate == 0) {
+			/* we need at minimum 10 Hz to be able to see anything */
+			sleep_delay = 1000000 / 10;
+		}
+	}
+
+	param_t log_ext_ph = param_find("SDLOG_EXT");
+
+	if (log_ext_ph != PARAM_INVALID) {
+
+		int32_t param_log_extended;
+		param_get(log_ext_ph, &param_log_extended);
+
+		if (param_log_extended > 0) {
+			_extended_logging = true;
+		} else if (param_log_extended == 0) {
+			_extended_logging = false;
+		}
+		/* any other value means to ignore the parameter, so no else case */
+
+	}
+
 	/* create log root dir */
 	int mkdir_ret = mkdir(log_root, S_IRWXU | S_IRWXG | S_IRWXO);
 
@@ -834,8 +975,10 @@ int sdlog2_thread_main(int argc, char *argv[])
 			struct log_ESTM_s log_ESTM;
 			struct log_PWR_s log_PWR;
 			struct log_VICN_s log_VICN;
-			struct log_GSN0_s log_GSN0;
-			struct log_GSN1_s log_GSN1;
+			struct log_GS0A_s log_GS0A;
+			struct log_GS0B_s log_GS0B;
+			struct log_GS1A_s log_GS1A;
+			struct log_GS1B_s log_GS1B;
 		} body;
 	} log_msg = {
 		LOG_PACKET_HEADER_INIT(0)
@@ -969,8 +1112,17 @@ int sdlog2_thread_main(int argc, char *argv[])
 			LOGBUFFER_WRITE_AND_COUNT(STAT);
 		}
 
-		/* --- GPS POSITION --- */
+		/* --- GPS POSITION - UNIT #1 --- */
 		if (gps_pos_updated) {
+
+			float snr_mean = 0.0f;
+
+			for (unsigned i = 0; i < buf_gps_pos.satellites_visible; i++) {
+				snr_mean += buf_gps_pos.satellite_snr[i];
+			}
+
+			snr_mean /= buf_gps_pos.satellites_visible;
+
 			log_msg.msg_type = LOG_GPS_MSG;
 			log_msg.body.log_GPS.gps_time = buf_gps_pos.time_gps_usec;
 			log_msg.body.log_GPS.fix_type = buf_gps_pos.fix_type;
@@ -983,19 +1135,48 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_GPS.vel_e = buf_gps_pos.vel_e_m_s;
 			log_msg.body.log_GPS.vel_d = buf_gps_pos.vel_d_m_s;
 			log_msg.body.log_GPS.cog = buf_gps_pos.cog_rad;
+			log_msg.body.log_GPS.sats = buf_gps_pos.satellites_visible;
+			log_msg.body.log_GPS.snr_mean = snr_mean;
+			log_msg.body.log_GPS.noise_per_ms = buf_gps_pos.noise_per_ms;
+			log_msg.body.log_GPS.jamming_indicator = buf_gps_pos.jamming_indicator;
 			LOGBUFFER_WRITE_AND_COUNT(GPS);
 
-			/* log the SNR of each satellite for a detailed view of signal quality */
-			log_msg.msg_type = LOG_GSN0_MSG;
-			/* pick the smaller number so we do not overflow any of the arrays */
-			unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
-			unsigned log_max_snr = sizeof(log_msg.body.log_GSN0.satellite_snr) / sizeof(log_msg.body.log_GSN0.satellite_snr[0]);
-			unsigned sat_max_snr = (gps_msg_max_snr < log_max_snr) ? gps_msg_max_snr : log_max_snr;
+			if (_extended_logging) {
+				/* log the SNR of each satellite for a detailed view of signal quality */
+				unsigned gps_msg_max_snr = sizeof(buf_gps_pos.satellite_snr) / sizeof(buf_gps_pos.satellite_snr[0]);
+				unsigned log_max_snr = sizeof(log_msg.body.log_GS0A.satellite_snr) / sizeof(log_msg.body.log_GS0A.satellite_snr[0]);
+
+				log_msg.msg_type = LOG_GS0A_MSG;
+				memset(&log_msg.body.log_GS0A, 0, sizeof(log_msg.body.log_GS0A));
+				/* fill set A */
+				for (unsigned i = 0; i < gps_msg_max_snr; i++) {
+
+					int satindex = buf_gps_pos.satellite_prn[i] - 1;
 
-			for (unsigned i = 0; i < sat_max_snr; i++) {
-				log_msg.body.log_GSN0.satellite_snr[i] = buf_gps_pos.satellite_snr[i];
+					/* handles index exceeding and wraps to to arithmetic errors */
+					if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
+						/* map satellites by their ID so that logs from two receivers can be compared */
+						log_msg.body.log_GS0A.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
+					}
+				}
+				LOGBUFFER_WRITE_AND_COUNT(GS0A);
+
+				log_msg.msg_type = LOG_GS0B_MSG;
+				memset(&log_msg.body.log_GS0B, 0, sizeof(log_msg.body.log_GS0B));
+				/* fill set B */
+				for (unsigned i = 0; i < gps_msg_max_snr; i++) {
+
+					/* get second bank of satellites, thus deduct bank size from index */
+					int satindex = buf_gps_pos.satellite_prn[i] - 1 - log_max_snr;
+
+					/* handles index exceeding and wraps to to arithmetic errors */
+					if ((satindex >= 0) && (satindex < (int)log_max_snr)) {
+						/* map satellites by their ID so that logs from two receivers can be compared */
+						log_msg.body.log_GS0B.satellite_snr[satindex] = buf_gps_pos.satellite_snr[i];
+					}
+				}
+				LOGBUFFER_WRITE_AND_COUNT(GS0B);
 			}
-			LOGBUFFER_WRITE_AND_COUNT(GSN0);
 		}
 
 		/* --- SENSOR COMBINED --- */
@@ -1119,10 +1300,16 @@ int sdlog2_thread_main(int argc, char *argv[])
 			log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat * 1e7;
 			log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon * 1e7;
 			log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
-			log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
-			log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
+			log_msg.body.log_LPOS.pos_flags = (buf.local_pos.xy_valid ? 1 : 0) |
+											  (buf.local_pos.z_valid ? 2 : 0) |
+											  (buf.local_pos.v_xy_valid ? 4 : 0) |
+											  (buf.local_pos.v_z_valid ? 8 : 0) |
+											  (buf.local_pos.xy_global ? 16 : 0) |
+											  (buf.local_pos.z_global ? 32 : 0);
 			log_msg.body.log_LPOS.landed = buf.local_pos.landed;
 			log_msg.body.log_LPOS.ground_dist_flags = (buf.local_pos.dist_bottom_valid ? 1 : 0);
+			log_msg.body.log_LPOS.eph = buf.local_pos.eph;
+			log_msg.body.log_LPOS.epv = buf.local_pos.epv;
 			LOGBUFFER_WRITE_AND_COUNT(LPOS);
 		}
 
@@ -1334,6 +1521,7 @@ void sdlog2_status()
 	float seconds = ((float)(hrt_absolute_time() - start_time)) / 1000000.0f;
 
 	warnx("wrote %lu msgs, %4.2f MiB (average %5.3f KiB/s), skipped %lu msgs", log_msgs_written, (double)mebibytes, (double)(kibibytes / seconds), log_msgs_skipped);
+	warnx("extended logging: %s", (_extended_logging) ? "ON" : "OFF");
 	mavlink_log_info(mavlink_fd, "[sdlog2] wrote %lu msgs, skipped %lu msgs", log_msgs_written, log_msgs_skipped);
 }
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 0c6188657..f4d88f079 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -109,10 +109,11 @@ struct log_LPOS_s {
 	int32_t ref_lat;
 	int32_t ref_lon;
 	float ref_alt;
-	uint8_t xy_flags;
-	uint8_t z_flags;
+	uint8_t pos_flags;
 	uint8_t landed;
 	uint8_t ground_dist_flags;
+	float eph;
+	float epv;
 };
 
 /* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -138,6 +139,10 @@ struct log_GPS_s {
 	float vel_e;
 	float vel_d;
 	float cog;
+	uint8_t sats;
+	uint16_t snr_mean;
+	uint16_t noise_per_ms;
+	uint16_t jamming_indicator;
 };
 
 /* --- ATTC - ATTITUDE CONTROLS (ACTUATOR_0 CONTROLS)--- */
@@ -317,16 +322,28 @@ struct log_VICN_s {
 	float yaw;
 };
 
-/* --- GSN0 - GPS SNR #0 --- */
-#define LOG_GSN0_MSG 26
-struct log_GSN0_s {
-	uint8_t satellite_snr[16];			/**< Signal to noise ratio of satellite. 0 for none, 255 for max. */
+/* --- GS0A - GPS SNR #0, SAT GROUP A --- */
+#define LOG_GS0A_MSG 26
+struct log_GS0A_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
+};
+
+/* --- GS0B - GPS SNR #0, SAT GROUP B --- */
+#define LOG_GS0B_MSG 27
+struct log_GS0B_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
+};
+
+/* --- GS1A - GPS SNR #1, SAT GROUP A --- */
+#define LOG_GS1A_MSG 28
+struct log_GS1A_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
 };
 
-/* --- GSN1 - GPS SNR #1 --- */
-#define LOG_GSN1_MSG 27
-struct log_GSN1_s {
-	uint8_t satellite_snr[16];			/**< Signal to noise ratio of satellite. 0 for none, 255 for max. */
+/* --- GS1B - GPS SNR #1, SAT GROUP B --- */
+#define LOG_GS1B_MSG 29
+struct log_GS1B_s {
+	uint8_t satellite_snr[16];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99 */
 };
 
 /********** SYSTEM MESSAGES, ID > 0x80 **********/
@@ -360,9 +377,9 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ATSP, "ffff",		"RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff",		"AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "fffff",		"BaroPres,BaroAlt,BaroTemp,DiffPres,DiffPresFilt"),
-	LOG_FORMAT(LPOS, "ffffffffLLfBBBB",	"X,Y,Z,dist,distR,VX,VY,VZ,RLat,RLon,RAlt,XYFlg,ZFlg,LFlg,GFlg"),
+	LOG_FORMAT(LPOS, "ffffffffLLfBBBff",	"X,Y,Z,Dist,DistR,VX,VY,VZ,RLat,RLon,RAlt,PFlg,LFlg,GFlg,EPH,EPV"),
 	LOG_FORMAT(LPSP, "ffff",		"X,Y,Z,Yaw"),
-	LOG_FORMAT(GPS, "QBffLLfffff",		"GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
+	LOG_FORMAT(GPS, "QBffLLfffffBHHH",	"GPSTime,Fix,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog,nSat,SNR,N,J"),
 	LOG_FORMAT(ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
 	LOG_FORMAT(STAT, "BBBfBB",		"MainState,ArmState,FailsafeState,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffffBB",		"Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7,Count,SignalLost"),
@@ -380,8 +397,10 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ESTM, "ffffffffffBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,nStat,statNaN,covNaN,kGainNaN"),
 	LOG_FORMAT(PWR, "fffBBBBB",		"Periph5V,Servo5V,RSSI,UsbOk,BrickOk,ServoOk,PeriphOC,HipwrOC"),
 	LOG_FORMAT(VICN, "ffffff",		"X,Y,Z,Roll,Pitch,Yaw"),
-	LOG_FORMAT(GSN0, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(GSN1, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
+	LOG_FORMAT(GS0A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
+	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"),
 
 	/* system-level messages, ID >= 0x80 */
 	/* FMT: don't write format of format message, it's useless */
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index c021e1fde..999cf8bb3 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -626,6 +626,15 @@ PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);
 
 /**
+ * Acro switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
+
+/**
  * Flaps channel mapping.
  *
  * @min 0
@@ -756,3 +765,19 @@ PARAM_DEFINE_FLOAT(RC_RETURN_TH, 0.5f);
  *
  */
 PARAM_DEFINE_FLOAT(RC_LOITER_TH, 0.5f);
+
+/**
+ * Threshold for selecting acro mode
+ *
+ * min:-1
+ * max:+1
+ *
+ * 0-1 indicate where in the full channel range the threshold sits
+ * 		0 : min
+ * 		1 : max
+ * sign indicates polarity of comparison
+ * 		positive : true when channel>th
+ * 		negative : true when channel<th
+ *
+ */
+PARAM_DEFINE_FLOAT(RC_ACRO_TH, 0.5f);
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index c5954d02a..b268b1b36 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -263,6 +263,7 @@ private:
 		int rc_map_return_sw;
 		int rc_map_posctl_sw;
 		int rc_map_loiter_sw;
+		int rc_map_acro_sw;
 
 		int rc_map_flaps;
 
@@ -278,11 +279,13 @@ private:
 		float rc_posctl_th;
 		float rc_return_th;
 		float rc_loiter_th;
+		float rc_acro_th;
 		bool rc_assist_inv;
 		bool rc_auto_inv;
 		bool rc_posctl_inv;
 		bool rc_return_inv;
 		bool rc_loiter_inv;
+		bool rc_acro_inv;
 
 		float battery_voltage_scaling;
 		float battery_current_scaling;
@@ -315,6 +318,7 @@ private:
 		param_t rc_map_return_sw;
 		param_t rc_map_posctl_sw;
 		param_t rc_map_loiter_sw;
+		param_t rc_map_acro_sw;
 
 		param_t rc_map_flaps;
 
@@ -330,6 +334,7 @@ private:
 		param_t rc_posctl_th;
 		param_t rc_return_th;
 		param_t rc_loiter_th;
+		param_t rc_acro_th;
 
 		param_t battery_voltage_scaling;
 		param_t battery_current_scaling;
@@ -530,6 +535,7 @@ Sensors::Sensors() :
 	/* optional mode switches, not mapped per default */
 	_parameter_handles.rc_map_posctl_sw = param_find("RC_MAP_POSCTL_SW");
 	_parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW");
+	_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
 
 	_parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1");
 	_parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2");
@@ -544,6 +550,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_posctl_th = param_find("RC_POSCTL_TH");
 	_parameter_handles.rc_return_th = param_find("RC_RETURN_TH");
 	_parameter_handles.rc_loiter_th = param_find("RC_LOITER_TH");
+	_parameter_handles.rc_acro_th = param_find("RC_ACRO_TH");
 
 	/* gyro offsets */
 	_parameter_handles.gyro_offset[0] = param_find("SENS_GYRO_XOFF");
@@ -687,6 +694,10 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}
 
+	if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) {
+		warnx("%s", paramerr);
+	}
+
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
 		warnx("%s", paramerr);
 	}
@@ -712,6 +723,9 @@ Sensors::parameters_update()
 	param_get(_parameter_handles.rc_loiter_th, &(_parameters.rc_loiter_th));
 	_parameters.rc_loiter_inv = (_parameters.rc_loiter_th < 0);
 	_parameters.rc_loiter_th = fabs(_parameters.rc_loiter_th);
+	param_get(_parameter_handles.rc_acro_th, &(_parameters.rc_acro_th));
+	_parameters.rc_acro_inv = (_parameters.rc_acro_th < 0);
+	_parameters.rc_acro_th = fabs(_parameters.rc_acro_th);
 
 	/* update RC function mappings */
 	_rc.function[THROTTLE] = _parameters.rc_map_throttle - 1;
@@ -723,6 +737,7 @@ Sensors::parameters_update()
 	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
 	_rc.function[POSCTL] = _parameters.rc_map_posctl_sw - 1;
 	_rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1;
+	_rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
 
@@ -1508,6 +1523,7 @@ Sensors::rc_poll()
 			manual.posctl_switch = get_rc_sw2pos_position(POSCTL, _parameters.rc_posctl_th, _parameters.rc_posctl_inv);
 			manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv);
 			manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv);
+			manual.acro_switch = get_rc_sw2pos_position(ACRO, _parameters.rc_acro_th, _parameters.rc_acro_inv);
 
 			/* publish manual_control_setpoint topic */
 			if (_manual_control_pub > 0) {
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 1c889a811..225570fa4 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -447,6 +447,7 @@ public:
 		QUAD_WIDE,	/**< quad in wide configuration */
 		HEX_X,		/**< hex in X configuration */
 		HEX_PLUS,	/**< hex in + configuration */
+		HEX_COX,
 		OCTA_X,
 		OCTA_PLUS,
 		OCTA_COX,
@@ -516,7 +517,7 @@ private:
 	float				_roll_scale;
 	float				_pitch_scale;
 	float				_yaw_scale;
-	float				_deadband;
+	float				_idle_speed;
 
 	unsigned			_rotor_count;
 	const Rotor			*_rotors;
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index bf77795d5..092c0e2b0 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -67,6 +67,11 @@
 namespace
 {
 
+float constrain(float val, float min, float max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
 /*
  * These tables automatically generated by multi_tables - do not edit.
  */
@@ -110,6 +115,14 @@ const MultirotorMixer::Rotor _config_hex_plus[] = {
 	{  0.866025,  0.500000,  1.00 },
 	{ -0.866025, -0.500000, -1.00 },
 };
+const MultirotorMixer::Rotor _config_hex_cox[] = {
+	{ -0.866025,  0.500000, -1.00 },
+	{ -0.866025,  0.500000,  1.00 },
+	{ -0.000000, -1.000000, -1.00 },
+	{ -0.000000, -1.000000,  1.00 },
+	{  0.866025,  0.500000, -1.00 },
+	{  0.866025,  0.500000,  1.00 },
+};
 const MultirotorMixer::Rotor _config_octa_x[] = {
 	{ -0.382683,  0.923880, -1.00 },
 	{  0.382683, -0.923880, -1.00 },
@@ -147,6 +160,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_quad_wide[0],
 	&_config_hex_x[0],
 	&_config_hex_plus[0],
+	&_config_hex_cox[0],
 	&_config_octa_x[0],
 	&_config_octa_plus[0],
 	&_config_octa_cox[0],
@@ -158,6 +172,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
 	4, /* quad_wide */
 	6, /* hex_x */
 	6, /* hex_plus */
+	6, /* hex_cox */
 	8, /* octa_x */
 	8, /* octa_plus */
 	8, /* octa_cox */
@@ -171,12 +186,12 @@ MultirotorMixer::MultirotorMixer(ControlCallback control_cb,
 				 float roll_scale,
 				 float pitch_scale,
 				 float yaw_scale,
-				 float deadband) :
+				 float idle_speed) :
 	Mixer(control_cb, cb_handle),
 	_roll_scale(roll_scale),
 	_pitch_scale(pitch_scale),
 	_yaw_scale(yaw_scale),
-	_deadband(-1.0f + deadband),	/* shift to output range here to avoid runtime calculation */
+	_idle_speed(-1.0f + idle_speed * 2.0f),	/* shift to output range here to avoid runtime calculation */
 	_rotor_count(_config_rotor_count[geometry]),
 	_rotors(_config_index[geometry])
 {
@@ -247,6 +262,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 	} else if (!strcmp(geomname, "6x")) {
 		geometry = MultirotorMixer::HEX_X;
 
+	} else if (!strcmp(geomname, "6c")) {
+		geometry = MultirotorMixer::HEX_COX;
+
 	} else if (!strcmp(geomname, "8+")) {
 		geometry = MultirotorMixer::OCTA_PLUS;
 
@@ -276,67 +294,67 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 unsigned
 MultirotorMixer::mix(float *outputs, unsigned space)
 {
-	float		roll    = get_control(0, 0) * _roll_scale;
+	float		roll    = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
 	//lowsyslog("roll: %d, get_control0: %d, %d\n", (int)(roll), (int)(get_control(0, 0)), (int)(_roll_scale));
-	float		pitch   = get_control(0, 1) * _pitch_scale;
-	float		yaw     = get_control(0, 2) * _yaw_scale;
-	float		thrust  = get_control(0, 3);
+	float		pitch   = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
+	float		yaw     = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
+	float		thrust  = constrain(get_control(0, 3), 0.0f, 1.0f);
 	//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
-	float		max     = 0.0f;
-	float		fixup_scale;
+	float		min_out = 0.0f;
+	float		max_out = 0.0f;
+	float		scale_yaw = 1.0f;
 
-	/* use an output factor to prevent too strong control signals at low throttle */
-	float min_thrust = 0.05f;
-	float max_thrust = 1.0f;
-	float startpoint_full_control = 0.40f;
-	float output_factor;
+	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		float out = roll * _rotors[i].roll_scale +
+			    pitch * _rotors[i].pitch_scale +
+			    thrust;
 
-	/* keep roll, pitch and yaw control to 0 below min thrust */
-	if (thrust <= min_thrust) {
-		output_factor = 0.0f;
-		/* linearly increase the output factor from 0 to 1 between min_thrust and startpoint_full_control */
+		/* limit yaw if it causes outputs clipping */
+		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
+			yaw = -out / _rotors[i].yaw_scale;
+		}
 
-	} else if (thrust < startpoint_full_control && thrust > min_thrust) {
-		output_factor = (thrust / max_thrust) / (startpoint_full_control - min_thrust);
-		/* and then stay at full control */
+		/* calculate min and max output values */
+		if (out < min_out) {
+			min_out = out;
+		}
+		if (out > max_out) {
+			max_out = out;
+		}
 
-	} else {
-		output_factor = max_thrust;
+		outputs[i] = out;
 	}
 
-	roll *= output_factor;
-	pitch *= output_factor;
-	yaw *= output_factor;
-
-
-	/* perform initial mix pass yielding un-bounded outputs */
-	for (unsigned i = 0; i < _rotor_count; i++) {
-		float tmp = roll  * _rotors[i].roll_scale +
-			    pitch * _rotors[i].pitch_scale +
-			    yaw   * _rotors[i].yaw_scale +
-			    thrust;
+	/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
+	if (min_out < 0.0) {
+		float scale_in = thrust / (thrust - min_out);
 
-		if (tmp > max)
-			max = tmp;
+		/* mix again with adjusted controls */
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
+		}
 
-		outputs[i] = tmp;
+	} else {
+		/* roll/pitch mixed without limiting, add yaw control */
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs[i] += yaw * _rotors[i].yaw_scale;
+		}
 	}
 
-	/* scale values into the -1.0 - 1.0 range */
-	if (max > 1.0f) {
-		fixup_scale = 2.0f / max;
+	/* scale down all outputs if some outputs are too large, reduce total thrust */
+	float scale_out;
+	if (max_out > 1.0f) {
+		scale_out = 1.0f / max_out;
 
 	} else {
-		fixup_scale = 2.0f;
+		scale_out = 1.0f;
 	}
 
-	for (unsigned i = 0; i < _rotor_count; i++)
-		outputs[i] = -1.0f + (outputs[i] * fixup_scale);
-
-	/* ensure outputs are out of the deadband */
-	for (unsigned i = 0; i < _rotor_count; i++)
-		if (outputs[i] < _deadband)
-			outputs[i] = _deadband;
+	/* scale outputs to range _idle_speed..1, and do final limiting */
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] = constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out), _idle_speed, 1.0f);
+	}
 
 	return _rotor_count;
 }
diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables
index 050bf2f47..b5698036e 100755
--- a/src/modules/systemlib/mixer/multi_tables
+++ b/src/modules/systemlib/mixer/multi_tables
@@ -52,6 +52,15 @@ set hex_plus {
 	 120 CW
 }
 
+set hex_cox {
+	  60 CW
+	  60 CCW
+	 180 CW
+	 180 CCW
+	 -60 CW
+	 -60 CCW
+}
+
 set octa_x {
 	  22.5 CW
 	-157.5 CW
@@ -85,7 +94,7 @@ set octa_cox {
 	-135 CW
 }
 
-set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus octa_cox}
+set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
 
 proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
 
@@ -104,13 +113,13 @@ foreach table $tables {
 	puts "};"
 }
 
-puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {"
+puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::MAX_GEOMETRY\] = {"
 foreach table $tables {
 	puts [format "\t&_config_%s\[0\]," $table]
 }
 puts "};"
 
-puts "const unsigned _config_rotor_count\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {"
+puts "const unsigned _config_rotor_count\[MultirotorMixer::MAX_GEOMETRY\] = {"
 foreach table $tables {
 	upvar #0 $table angles
 	puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table]
diff --git a/src/modules/systemlib/perf_counter.c b/src/modules/systemlib/perf_counter.c
index b4ca0ed3e..22182e39e 100644
--- a/src/modules/systemlib/perf_counter.c
+++ b/src/modules/systemlib/perf_counter.c
@@ -282,12 +282,18 @@ perf_reset(perf_counter_t handle)
 void
 perf_print_counter(perf_counter_t handle)
 {
+	perf_print_counter_fd(0, handle);
+}
+
+void
+perf_print_counter_fd(int fd, perf_counter_t handle)
+{
 	if (handle == NULL)
 		return;
 
 	switch (handle->type) {
 	case PC_COUNT:
-		printf("%s: %llu events\n",
+		dprintf(fd, "%s: %llu events\n",
 		       handle->name,
 		       ((struct perf_ctr_count *)handle)->event_count);
 		break;
@@ -295,7 +301,7 @@ perf_print_counter(perf_counter_t handle)
 	case PC_ELAPSED: {
 		struct perf_ctr_elapsed *pce = (struct perf_ctr_elapsed *)handle;
 
-		printf("%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %lluus elapsed, %llu avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pce->event_count,
 		       pce->time_total,
@@ -308,7 +314,7 @@ perf_print_counter(perf_counter_t handle)
 	case PC_INTERVAL: {
 		struct perf_ctr_interval *pci = (struct perf_ctr_interval *)handle;
 
-		printf("%s: %llu events, %llu avg, min %lluus max %lluus\n",
+		dprintf(fd, "%s: %llu events, %llu avg, min %lluus max %lluus\n",
 		       handle->name,
 		       pci->event_count,
 		       (pci->time_last - pci->time_first) / pci->event_count,
@@ -349,12 +355,12 @@ perf_event_count(perf_counter_t handle)
 }
 
 void
-perf_print_all(void)
+perf_print_all(int fd)
 {
 	perf_counter_t handle = (perf_counter_t)sq_peek(&perf_counters);
 
 	while (handle != NULL) {
-		perf_print_counter(handle);
+		perf_print_counter_fd(fd, handle);
 		handle = (perf_counter_t)sq_next(&handle->link);
 	}
 }
diff --git a/src/modules/systemlib/perf_counter.h b/src/modules/systemlib/perf_counter.h
index d8f69fdbf..6835ee4a2 100644
--- a/src/modules/systemlib/perf_counter.h
+++ b/src/modules/systemlib/perf_counter.h
@@ -121,16 +121,26 @@ __EXPORT extern void		perf_cancel(perf_counter_t handle);
 __EXPORT extern void		perf_reset(perf_counter_t handle);
 
 /**
- * Print one performance counter.
+ * Print one performance counter to stdout
  *
  * @param handle		The counter to print.
  */
 __EXPORT extern void		perf_print_counter(perf_counter_t handle);
 
 /**
+ * Print one performance counter to a fd.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
+ * @param handle		The counter to print.
+ */
+__EXPORT extern void		perf_print_counter_fd(int fd, perf_counter_t handle);
+
+/**
  * Print all of the performance counters.
+ *
+ * @param fd			File descriptor to print to - e.g. 0 for stdout
  */
-__EXPORT extern void		perf_print_all(void);
+__EXPORT extern void		perf_print_all(int fd);
 
 /**
  * Reset all of the performance counters.
diff --git a/src/modules/systemlib/pwm_limit/pwm_limit.c b/src/modules/systemlib/pwm_limit/pwm_limit.c
index 190b315f1..ea5ba9e52 100644
--- a/src/modules/systemlib/pwm_limit/pwm_limit.c
+++ b/src/modules/systemlib/pwm_limit/pwm_limit.c
@@ -136,12 +136,26 @@ void pwm_limit_calc(const bool armed, const unsigned num_channels, const uint16_
 					}
 
 					effective_pwm[i] = output[i] * (max_pwm[i] - ramp_min_pwm)/2 + (max_pwm[i] + ramp_min_pwm)/2;
+
+					/* last line of defense against invalid inputs */
+					if (effective_pwm[i] < ramp_min_pwm) {
+						effective_pwm[i] = ramp_min_pwm;
+					} else if (effective_pwm[i] > max_pwm[i]) {
+						effective_pwm[i] = max_pwm[i];
+					}
 				}
 			}
 			break;
 		case PWM_LIMIT_STATE_ON:
 			for (unsigned i=0; i<num_channels; i++) {
 				effective_pwm[i] = output[i] * (max_pwm[i] - min_pwm[i])/2 + (max_pwm[i] + min_pwm[i])/2;
+
+				/* last line of defense against invalid inputs */
+				if (effective_pwm[i] < min_pwm[i]) {
+					effective_pwm[i] = min_pwm[i];
+				} else if (effective_pwm[i] > max_pwm[i]) {
+					effective_pwm[i] = max_pwm[i];
+				}
 			}
 			break;
 		default:
diff --git a/src/modules/uORB/topics/actuator_armed.h b/src/modules/uORB/topics/actuator_armed.h
index 6e944ffee..a98d3fc3a 100644
--- a/src/modules/uORB/topics/actuator_armed.h
+++ b/src/modules/uORB/topics/actuator_armed.h
@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -44,15 +44,25 @@
 #include <stdint.h>
 #include "../uORB.h"
 
+/**
+ * @addtogroup topics
+ * @{
+ */
+
 /** global 'actuator output is live' control. */
 struct actuator_armed_s {
 
-	uint64_t	timestamp;
-	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) */
+	uint64_t	timestamp;	/**< Microseconds since system boot */
+	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) */
 };
 
+/**
+ * @}
+ */
+
+/* register this as object request broker structure */
 ORB_DECLARE(actuator_armed);
 
 #endif
\ No newline at end of file
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index 19a29635b..910b8a623 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -97,6 +97,7 @@ struct manual_control_setpoint_s {
 	switch_pos_t return_switch;			/**< return to launch 2 position switch (mandatory): _NORMAL_, RTL */
 	switch_pos_t posctl_switch;			/**< position control 2 position switch (optional): _ALTCTL_, POSCTL */
 	switch_pos_t loiter_switch;			/**< loiter 2 position switch (optional): _MISSION_, LOITER */
+	switch_pos_t acro_switch;			/**< acro 2 position switch (optional): _MANUAL_, ACRO */
 }; /**< manual control inputs */
 
 /**
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index b60402452..370c54442 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -45,12 +45,12 @@
 /**
  * The number of RC channel inputs supported.
  * Current (Q4/2013) radios support up to 18 channels,
- * leaving at a sane value of 15.
+ * leaving at a sane value of 16.
  * This number can be greater then number of RC channels,
  * because single RC channel can be mapped to multiple
  * functions, e.g. for various mode switches.
  */
-#define RC_CHANNELS_MAPPED_MAX   15
+#define RC_CHANNELS_MAPPED_MAX   16
 
 /**
  * This defines the mapping of the RC functions.
@@ -67,12 +67,13 @@ enum RC_CHANNELS_FUNCTION {
 	POSCTL = 6,
 	LOITER = 7,
 	OFFBOARD_MODE = 8,
-	FLAPS   = 9,
-	AUX_1   = 10,
-	AUX_2   = 11,
-	AUX_3   = 12,
-	AUX_4   = 13,
-	AUX_5   = 14,
+	ACRO    = 9,
+	FLAPS   = 10,
+	AUX_1   = 11,
+	AUX_2   = 12,
+	AUX_3   = 13,
+	AUX_4   = 14,
+	AUX_5   = 15,
 	RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 
diff --git a/src/modules/uORB/topics/vehicle_gps_position.h b/src/modules/uORB/topics/vehicle_gps_position.h
index 794c3f8bc..5924a324d 100644
--- a/src/modules/uORB/topics/vehicle_gps_position.h
+++ b/src/modules/uORB/topics/vehicle_gps_position.h
@@ -68,6 +68,9 @@ struct vehicle_gps_position_s {
 	float eph_m;					/**< GPS HDOP horizontal dilution of position in m */
 	float epv_m;					/**< GPS VDOP horizontal dilution of position in m */
 
+	unsigned noise_per_ms;				/**< */
+	unsigned jamming_indicator;			/**< */
+
 	uint64_t timestamp_velocity;			/**< Timestamp for velocity informations */
 	float vel_m_s;					/**< GPS ground speed (m/s) */
 	float vel_n_m_s;				/**< GPS ground speed in m/s */
@@ -85,7 +88,7 @@ struct vehicle_gps_position_s {
 	uint8_t satellite_used[20];			/**< 0: Satellite not used, 1: used for localization */
 	uint8_t satellite_elevation[20]; 		/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
 	uint8_t satellite_azimuth[20];			/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
-	uint8_t satellite_snr[20];			/**< Signal to noise ratio of satellite   */
+	uint8_t satellite_snr[20];			/**< dBHz, Signal to noise ratio of satellite C/N0, range 0..99, zero when not tracking this satellite. */
 	bool satellite_info_available;			/**< 0 for no info, 1 for info available */
 };
 
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index a15303ea4..5d39c897d 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -83,6 +83,8 @@ struct vehicle_local_position_s {
 	float dist_bottom_rate;		/**< Distance to bottom surface (ground) change rate */
 	uint64_t surface_bottom_timestamp;		/**< Time when new bottom surface found */
 	bool dist_bottom_valid;	/**< true if distance to bottom surface is valid */
+	float eph;
+	float epv;
 };
 
 /**
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index f56355246..ea20a317a 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -66,6 +66,7 @@ typedef enum {
 	MAIN_STATE_ALTCTL,
 	MAIN_STATE_POSCTL,
 	MAIN_STATE_AUTO,
+	MAIN_STATE_ACRO,
 	MAIN_STATE_MAX
 } main_state_t;