Merge branch 'master' of github.com:PX4/Firmware into fixedwing_l1

29 files changed, 351 insertions, 291 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 65abcde1e..a70a14fe4 100755
--- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
+++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp
@@ -308,18 +308,20 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 				orb_copy(ORB_ID(sensor_combined), sub_raw, &raw);
 
 				if (!initialized) {
-
-					gyro_offsets[0] += raw.gyro_rad_s[0];
-					gyro_offsets[1] += raw.gyro_rad_s[1];
-					gyro_offsets[2] += raw.gyro_rad_s[2];
-					offset_count++;
-
-					if (hrt_absolute_time() - start_time > 3000000LL) {
-						initialized = true;
-						gyro_offsets[0] /= offset_count;
-						gyro_offsets[1] /= offset_count;
-						gyro_offsets[2] /= offset_count;
-					}
+					// XXX disabling init for now
+					initialized = true;
+
+					// gyro_offsets[0] += raw.gyro_rad_s[0];
+					// gyro_offsets[1] += raw.gyro_rad_s[1];
+					// gyro_offsets[2] += raw.gyro_rad_s[2];
+					// offset_count++;
+
+					// if (hrt_absolute_time() - start_time > 3000000LL) {
+					// 	initialized = true;
+					// 	gyro_offsets[0] /= offset_count;
+					// 	gyro_offsets[1] /= offset_count;
+					// 	gyro_offsets[2] /= offset_count;
+					// }
 
 				} else {
 
diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
index 236052b56..86bda3c75 100755
--- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
+++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp
@@ -730,7 +730,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					// Because proper mount of PX4 will give you a reversed accelerometer readings.
 					NonlinearSO3AHRSupdate(gyro[0],gyro[1],gyro[2],-acc[0],-acc[1],-acc[2],mag[0],mag[1],mag[2],so3_comp_params.Kp,so3_comp_params.Ki, dt);
 
-					// Convert q->R.
+					// Convert q->R, This R converts inertial frame to body frame.
 					Rot_matrix[0] = q0q0 + q1q1 - q2q2 - q3q3;// 11
         				Rot_matrix[1] = 2.0 * (q1*q2 + q0*q3);	// 12
         				Rot_matrix[2] = 2.0 * (q1*q3 - q0*q2);	// 13
@@ -794,7 +794,7 @@ const unsigned int loop_interval_alarm = 6500;	// loop interval in microseconds
 					memcpy(&att.rate_offsets, &(gyro_bias), sizeof(att.rate_offsets));
 
 					/* copy rotation matrix */
-					memcpy(&att.R, Rot_matrix, sizeof(Rot_matrix));
+					memcpy(&att.R, Rot_matrix, sizeof(float)*9);
 					att.R_valid = true;
 
 					if (isfinite(att.roll) && isfinite(att.pitch) && isfinite(att.yaw)) {
diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp
index ed6707f04..cfa7d9e8a 100644
--- a/src/modules/commander/accelerometer_calibration.cpp
+++ b/src/modules/commander/accelerometer_calibration.cpp
@@ -241,8 +241,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 			break;
 
 		int orient = detect_orientation(mavlink_fd, sensor_combined_sub);
-		if (orient < 0)
+		if (orient < 0) {
+			close(sensor_combined_sub);
 			return ERROR;
+		}
 
 		if (data_collected[orient]) {
 			mavlink_log_info(mavlink_fd, "%s done, please rotate to a different axis", orientation_strs[orient]);
diff --git a/src/modules/commander/airspeed_calibration.cpp b/src/modules/commander/airspeed_calibration.cpp
index e414e5f70..248eb4a66 100644
--- a/src/modules/commander/airspeed_calibration.cpp
+++ b/src/modules/commander/airspeed_calibration.cpp
@@ -85,6 +85,7 @@ int do_airspeed_calibration(int mavlink_fd)
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
 			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
+			close(diff_pres_sub);
 			return ERROR;
 		}
 	}
@@ -95,6 +96,7 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
 			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
+			close(diff_pres_sub);
 			return ERROR;
 		}
 
@@ -104,18 +106,17 @@ int do_airspeed_calibration(int mavlink_fd)
 		if (save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
 			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
+			close(diff_pres_sub);
 			return ERROR;
 		}
 
-		//char buf[50];
-		//sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]);
-		//mavlink_log_info(mavlink_fd, buf);
 		mavlink_log_info(mavlink_fd, "airspeed calibration done");
-
+		close(diff_pres_sub);
 		return OK;
 
 	} else {
 		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+		close(diff_pres_sub);
 		return ERROR;
 	}
 }
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index a548f943e..333fe30ae 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -50,6 +50,7 @@
 #include <unistd.h>
 #include <fcntl.h>
 #include <errno.h>
+#include <systemlib/err.h>
 #include <debug.h>
 #include <sys/prctl.h>
 #include <sys/stat.h>
@@ -153,6 +154,7 @@ static uint64_t last_print_mode_reject_time = 0;
 /* if connected via USB */
 static bool on_usb_power = false;
 
+static float takeoff_alt = 5.0f;
 
 /* tasks waiting for low prio thread */
 typedef enum {
@@ -318,6 +320,8 @@ void print_status()
 		break;
 	}
 
+	close(state_sub);
+
 
 	warnx("arming: %s", armed_str);
 }
@@ -492,9 +496,10 @@ int commander_thread_main(int argc, char *argv[])
 	param_t _param_sys_type = param_find("MAV_TYPE");
 	param_t _param_system_id = param_find("MAV_SYS_ID");
 	param_t _param_component_id = param_find("MAV_COMP_ID");
+	param_t _param_takeoff_alt = param_find("NAV_TAKEOFF_ALT");
 
 	/* welcome user */
-	warnx("[commander] starting");
+	warnx("starting");
 
 	/* pthread for slow low prio thread */
 	pthread_t commander_low_prio_thread;
@@ -592,9 +597,10 @@ int commander_thread_main(int argc, char *argv[])
 
 	pthread_attr_t commander_low_prio_attr;
 	pthread_attr_init(&commander_low_prio_attr);
-	pthread_attr_setstacksize(&commander_low_prio_attr, 2048);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2992);
 
 	struct sched_param param;
+	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
 	/* low priority */
 	param.sched_priority = SCHED_PRIORITY_DEFAULT - 50;
 	(void)pthread_attr_setschedparam(&commander_low_prio_attr, &param);
@@ -689,6 +695,8 @@ int commander_thread_main(int argc, char *argv[])
 	struct subsystem_info_s info;
 	memset(&info, 0, sizeof(info));
 
+	toggle_status_leds(&status, &armed, true);
+
 	/* now initialized */
 	commander_initialized = true;
 	thread_running = true;
@@ -731,8 +739,11 @@ int commander_thread_main(int argc, char *argv[])
 				param_get(_param_component_id, &(status.component_id));
 				status_changed = true;
 
-				/* Re-check RC calibration */
+				/* re-check RC calibration */
 				rc_calibration_ok = (OK == rc_calibration_check());
+
+				/* navigation parameters */
+				param_get(_param_takeoff_alt, &takeoff_alt);
 			}
 		}
 
@@ -890,7 +901,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) {
 				low_battery_voltage_actions_done = true;
 				mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY");
-				status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING;
+				status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
 				status_changed = true;
 				battery_tune_played = false;
 			}
@@ -902,7 +913,7 @@ int commander_thread_main(int argc, char *argv[])
 			if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) {
 				critical_battery_voltage_actions_done = true;
 				mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY");
-				status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT;
+				status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 				battery_tune_played = false;
 
 				if (armed.armed) {
@@ -1160,12 +1171,12 @@ int commander_thread_main(int argc, char *argv[])
 			if (tune_arm() == OK)
 				arm_tune_played = true;
 
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
 			/* play tune on battery warning */
 			if (tune_low_bat() == OK)
 				battery_tune_played = true;
 
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
 			/* play tune on battery critical */
 			if (tune_critical_bat() == OK)
 				battery_tune_played = true;
@@ -1251,73 +1262,43 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chang
 #endif
 
 	if (changed) {
-
-		int i;
-		rgbled_pattern_t pattern;
-		memset(&pattern, 0, sizeof(pattern));
+		/* XXX TODO blink fast when armed and serious error occurs */
 
 		if (armed->armed) {
-			/* armed, solid */
-			if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
-				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
-
-			} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
-				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
-
-			} else {
-				pattern.color[0] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
-			}
-
-			pattern.duration[0] = 1000;
-
+			rgbled_set_mode(RGBLED_MODE_ON);
 		} else if (armed->ready_to_arm) {
-			for (i = 0; i < 3; i++) {
-				if (status->battery_warning == VEHICLE_BATTERY_WARNING_WARNING) {
-					pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_AMBER : RGBLED_COLOR_AMBER;
-
-				} else if (status->battery_warning == VEHICLE_BATTERY_WARNING_ALERT) {
-					pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
-
-				} else {
-					pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_GREEN : RGBLED_COLOR_GREEN;
-				}
-
-				pattern.duration[i * 2] = 200;
-
-				pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
-				pattern.duration[i * 2 + 1] = 800;
-			}
-
-			if (status->condition_global_position_valid) {
-				pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
-				pattern.duration[i * 2] = 1000;
-				pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
-				pattern.duration[i * 2 + 1] = 800;
-
-			} else {
-				for (i = 3; i < 6; i++) {
-					pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_BLUE : RGBLED_COLOR_BLUE;
-					pattern.duration[i * 2] = 100;
-					pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
-					pattern.duration[i * 2 + 1] = 100;
-				}
-
-				pattern.color[6 * 2] = RGBLED_COLOR_OFF;
-				pattern.duration[6 * 2] = 700;
-			}
-
+			rgbled_set_mode(RGBLED_MODE_BREATHE);
 		} else {
-			for (i = 0; i < 3; i++) {
-				pattern.color[i * 2] = (on_usb_power) ? RGBLED_COLOR_DIM_RED : RGBLED_COLOR_RED;
-				pattern.duration[i * 2] = 200;
-				pattern.color[i * 2 + 1] = RGBLED_COLOR_OFF;
-				pattern.duration[i * 2 + 1] = 200;
-			}
-
-			/* not ready to arm, blink at 10Hz */
+			rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
 		}
+	}
 
-		rgbled_set_pattern(&pattern);
+	if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
+		switch (status->battery_warning) {
+			case VEHICLE_BATTERY_WARNING_LOW:
+				rgbled_set_color(RGBLED_COLOR_YELLOW);
+				break;
+			case VEHICLE_BATTERY_WARNING_CRITICAL:
+				rgbled_set_color(RGBLED_COLOR_AMBER);
+				break;
+			default:
+				break;
+		}
+	} else {
+		switch (status->main_state) {
+			case MAIN_STATE_MANUAL:
+				rgbled_set_color(RGBLED_COLOR_WHITE);
+				break;
+			case MAIN_STATE_SEATBELT:
+			case MAIN_STATE_EASY:
+				rgbled_set_color(RGBLED_COLOR_GREEN);
+				break;
+			case MAIN_STATE_AUTO:
+				rgbled_set_color(RGBLED_COLOR_BLUE);
+				break;
+			default:
+				break;
+		}
 	}
 
 	/* give system warnings on error LED, XXX maybe add memory usage warning too */
@@ -1480,54 +1461,52 @@ check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_c
 
 	if (status->main_state == MAIN_STATE_AUTO) {
 		if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) {
+			// TODO AUTO_LAND handling
 			if (status->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) {
 				/* don't switch to other states until takeoff not completed */
-				if (local_pos->z > -5.0f || status->condition_landed) {
-					res = TRANSITION_NOT_CHANGED;
+				if (local_pos->z > -takeoff_alt || status->condition_landed) {
+					return TRANSITION_NOT_CHANGED;
 				}
 			}
-
-			if (res != TRANSITION_NOT_CHANGED) {
-				/* check again, state can be changed */
-				if (status->condition_landed) {
-					/* if landed: transitions only to AUTO_READY are allowed */
-					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
-					// TRANSITION_DENIED is not possible here
+			if (status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF &&
+				status->navigation_state != NAVIGATION_STATE_AUTO_LOITER &&
+				status->navigation_state != NAVIGATION_STATE_AUTO_MISSION &&
+				status->navigation_state != NAVIGATION_STATE_AUTO_RTL) {
+				/* possibly on ground, switch to TAKEOFF if needed */
+				if (local_pos->z > -takeoff_alt || status->condition_landed) {
+					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_TAKEOFF, control_mode);
+					return res;
+				}
+			}
+			/* switch to AUTO mode */
+			if (status->rc_signal_found_once && !status->rc_signal_lost) {
+				/* act depending on switches when manual control enabled */
+				if (status->return_switch == RETURN_SWITCH_RETURN) {
+					/* RTL */
+					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
 
 				} else {
-					/* not landed */
-					if (status->rc_signal_found_once && !status->rc_signal_lost) {
-						/* act depending on switches when manual control enabled */
-						if (status->return_switch == RETURN_SWITCH_RETURN) {
-							/* RTL */
-							res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_RTL, control_mode);
-
-						} else {
-							if (status->mission_switch == MISSION_SWITCH_MISSION) {
-								/* MISSION */
-								res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
-
-							} else {
-								/* LOITER */
-								res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
-							}
-						}
+					if (status->mission_switch == MISSION_SWITCH_MISSION) {
+						/* MISSION */
+						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
 
 					} else {
-						/* switch to MISSION in air when no RC control */
-						if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
-						    status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
-						    status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
-						    status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
-							res = TRANSITION_NOT_CHANGED;
-
-						} else {
-							res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
-						}
+						/* LOITER */
+						res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_LOITER, control_mode);
 					}
 				}
-			}
+			} else {
+				/* switch to MISSION when no RC control and first time in some AUTO mode  */
+				if (status->navigation_state == NAVIGATION_STATE_AUTO_LOITER ||
+					status->navigation_state == NAVIGATION_STATE_AUTO_MISSION ||
+					status->navigation_state == NAVIGATION_STATE_AUTO_RTL ||
+					status->navigation_state == NAVIGATION_STATE_AUTO_LAND) {
+					res = TRANSITION_NOT_CHANGED;
 
+				} else {
+					res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_MISSION, control_mode);
+				}
+			}
 		} else {
 			/* disarmed, always switch to AUTO_READY */
 			res = navigation_state_transition(status, NAVIGATION_STATE_AUTO_READY, control_mode);
@@ -1678,13 +1657,13 @@ void *commander_low_prio_loop(void *arg)
 
 				if (((int)(cmd.param1)) == 1) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					usleep(1000000);
+					usleep(100000);
 					/* reboot */
 					systemreset(false);
 
 				} else if (((int)(cmd.param1)) == 3) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
-					usleep(1000000);
+					usleep(100000);
 					/* reboot to bootloader */
 					systemreset(true);
 
@@ -1727,6 +1706,7 @@ void *commander_low_prio_loop(void *arg)
 				} else if ((int)(cmd.param4) == 1) {
 					/* RC calibration */
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
+					calib_ret = do_rc_calibration(mavlink_fd);
 
 				} else if ((int)(cmd.param5) == 1) {
 					/* accelerometer calibration */
@@ -1752,22 +1732,36 @@ void *commander_low_prio_loop(void *arg)
 		case VEHICLE_CMD_PREFLIGHT_STORAGE: {
 
 				if (((int)(cmd.param1)) == 0) {
-					if (0 == param_load_default()) {
+					int ret = param_load_default();
+					if (ret == OK) {
 						mavlink_log_info(mavlink_fd, "[cmd] parameters loaded");
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
 						mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR");
+						/* convenience as many parts of NuttX use negative errno */
+						if (ret < 0)
+							ret = -ret;
+
+						if (ret < 1000)
+							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
 
 				} else if (((int)(cmd.param1)) == 1) {
-					if (0 == param_save_default()) {
+					int ret = param_save_default();
+					if (ret == OK) {
 						mavlink_log_info(mavlink_fd, "[cmd] parameters saved");
 						answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 
 					} else {
 						mavlink_log_critical(mavlink_fd, "[cmd] parameters save error");
+						/* convenience as many parts of NuttX use negative errno */
+						if (ret < 0)
+							ret = -ret;
+
+						if (ret < 1000)
+							mavlink_log_critical(mavlink_fd, "[cmd] %s", strerror(ret));
 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
 				}
@@ -1789,5 +1783,7 @@ void *commander_low_prio_loop(void *arg)
 
 	}
 
+	close(cmd_sub);
+
 	return 0;
 }
diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp
index 5df5d8d0c..7feace2b4 100644
--- a/src/modules/commander/commander_helper.cpp
+++ b/src/modules/commander/commander_helper.cpp
@@ -99,44 +99,44 @@ void buzzer_deinit()
 
 void tune_error()
 {
-	ioctl(buzzer, TONE_SET_ALARM, 2);
+	ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
 }
 
 void tune_positive()
 {
-	ioctl(buzzer, TONE_SET_ALARM, 3);
+	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
 }
 
 void tune_neutral()
 {
-	ioctl(buzzer, TONE_SET_ALARM, 4);
+	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
 }
 
 void tune_negative()
 {
-	ioctl(buzzer, TONE_SET_ALARM, 5);
+	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
 }
 
 int tune_arm()
 {
-	return ioctl(buzzer, TONE_SET_ALARM, 12);
+	return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE);
 }
 
 int tune_low_bat()
 {
-	return ioctl(buzzer, TONE_SET_ALARM, 13);
+	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE);
 }
 
 int tune_critical_bat()
 {
-	return ioctl(buzzer, TONE_SET_ALARM, 14);
+	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
 }
 
 
 
 void tune_stop()
 {
-	ioctl(buzzer, TONE_SET_ALARM, 0);
+	ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
 }
 
 static int leds;
diff --git a/src/modules/commander/commander_params.c b/src/modules/commander/commander_params.c
index 0a1703b2e..40d0386d5 100644
--- a/src/modules/commander/commander_params.c
+++ b/src/modules/commander/commander_params.c
@@ -45,7 +45,8 @@
 #include <nuttx/config.h>
 #include <systemlib/param/param.h>
 
-PARAM_DEFINE_INT32(SYS_FAILSAFE_LL, 0);	/**< Go into low-level failsafe after 0 ms */
+PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 5.0f);
+PARAM_DEFINE_FLOAT(NAV_TAKEOFF_GAP, 3.0f);
 PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f);
 PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f);
diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp
index 33566d4e5..82a0349c9 100644
--- a/src/modules/commander/gyro_calibration.cpp
+++ b/src/modules/commander/gyro_calibration.cpp
@@ -60,12 +60,12 @@ int do_gyro_calibration(int mavlink_fd)
 {
 	mavlink_log_info(mavlink_fd, "Gyro calibration starting, do not move unit.");
 
-	const int calibration_count = 5000;
+	const unsigned calibration_count = 5000;
 
 	int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined));
 	struct sensor_combined_s raw;
 
-	int calibration_counter = 0;
+	unsigned calibration_counter = 0;
 	float gyro_offset[3] = {0.0f, 0.0f, 0.0f};
 
 	/* set offsets to zero */
@@ -101,6 +101,8 @@ int do_gyro_calibration(int mavlink_fd)
 			gyro_offset[1] += raw.gyro_rad_s[1];
 			gyro_offset[2] += raw.gyro_rad_s[2];
 			calibration_counter++;
+			if (calibration_counter % (calibration_count / 20) == 0)
+				mavlink_log_info(mavlink_fd, "gyro cal progress <%u> percent", (calibration_counter * 100) / calibration_count);
 
 		} else {
 			poll_errcount++;
@@ -108,6 +110,7 @@ int do_gyro_calibration(int mavlink_fd)
 
 		if (poll_errcount > 1000) {
 			mavlink_log_info(mavlink_fd, "ERROR: Failed reading gyro sensor");
+			close(sub_sensor_combined);
 			return ERROR;
 		}
 	}
@@ -147,24 +150,23 @@ int do_gyro_calibration(int mavlink_fd)
 		if (save_ret != 0) {
 			warnx("WARNING: auto-save of params to storage failed");
 			mavlink_log_critical(mavlink_fd, "gyro store failed");
+			close(sub_sensor_combined);
 			return ERROR;
 		}
 
-		mavlink_log_info(mavlink_fd, "gyro calibration done");
-
 		tune_neutral();
 		/* third beep by cal end routine */
 
 	} else {
 		mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)");
+		close(sub_sensor_combined);
 		return ERROR;
 	}
 
 
 	/*** --- SCALING --- ***/
 
-	mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x");
-	mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip.");
+	mavlink_log_info(mavlink_fd, "offset done. Rotate for scale 30x or wait 5s to skip.");
 	warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip.");
 
 	unsigned rotations_count = 30;
@@ -187,8 +189,8 @@ int do_gyro_calibration(int mavlink_fd)
 		/* abort this loop if not rotated more than 180 degrees within 5 seconds */
 		if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f)
 			&& (hrt_absolute_time() - start_time > 5 * 1e6)) {
-			mavlink_log_info(mavlink_fd, "gyro scale calibration skipped");
-			mavlink_log_info(mavlink_fd, "gyro calibration done");
+			mavlink_log_info(mavlink_fd, "scale skipped, gyro calibration done");
+			close(sub_sensor_combined);
 			return OK;
 		}
 
@@ -281,9 +283,11 @@ int do_gyro_calibration(int mavlink_fd)
 		mavlink_log_info(mavlink_fd, "gyro calibration done");
 
 		/* third beep by cal end routine */
+		close(sub_sensor_combined);
 		return OK;
 	} else {
 		mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)");
+		close(sub_sensor_combined);
 		return ERROR;
 	}
 }
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index e38616027..b0d4266be 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -142,7 +142,6 @@ int do_mag_calibration(int mavlink_fd)
 			axis_index++;
 
 			mavlink_log_info(mavlink_fd, "please rotate in a figure 8 or around %c axis.", axislabels[axis_index]);
-			mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_maxcount * 50) / calibration_counter);
 			tune_neutral();
 
 			axis_deadline += calibration_interval / 3;
@@ -152,14 +151,6 @@ int do_mag_calibration(int mavlink_fd)
 			break;
 		}
 
-		// int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time();
-
-		// if ((axis_left / 1000) == 0 && axis_left > 0) {
-		// 	char buf[50];
-		// 	sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]);
-		// 	mavlink_log_info(mavlink_fd, buf);
-		// }
-
 		int poll_ret = poll(fds, 1, 1000);
 
 		if (poll_ret > 0) {
@@ -169,30 +160,10 @@ int do_mag_calibration(int mavlink_fd)
 			y[calibration_counter] = mag.y;
 			z[calibration_counter] = mag.z;
 
-			/* get min/max values */
-
-			// if (mag.x < mag_min[0]) {
-			// 	mag_min[0] = mag.x;
-			// }
-			// else if (mag.x > mag_max[0]) {
-			// 	mag_max[0] = mag.x;
-			// }
-
-			// if (raw.magnetometer_ga[1] < mag_min[1]) {
-			// 	mag_min[1] = raw.magnetometer_ga[1];
-			// }
-			// else if (raw.magnetometer_ga[1] > mag_max[1]) {
-			// 	mag_max[1] = raw.magnetometer_ga[1];
-			// }
-
-			// if (raw.magnetometer_ga[2] < mag_min[2]) {
-			// 	mag_min[2] = raw.magnetometer_ga[2];
-			// }
-			// else if (raw.magnetometer_ga[2] > mag_max[2]) {
-			// 	mag_max[2] = raw.magnetometer_ga[2];
-			// }
-
 			calibration_counter++;
+			if (calibration_counter % (calibration_maxcount / 20) == 0)
+				mavlink_log_info(mavlink_fd, "mag cal progress <%u> percent", 20 + (calibration_counter * 50) / calibration_maxcount);
+
 
 		} else {
 			poll_errcount++;
@@ -200,6 +171,10 @@ int do_mag_calibration(int mavlink_fd)
 
 		if (poll_errcount > 1000) {
 			mavlink_log_info(mavlink_fd, "ERROR: Failed reading mag sensor");
+			close(sub_mag);
+			free(x);
+			free(y);
+			free(z);
 			return ERROR;
 		}
 
@@ -211,7 +186,9 @@ int do_mag_calibration(int mavlink_fd)
 	float sphere_z;
 	float sphere_radius;
 
+	mavlink_log_info(mavlink_fd, "mag cal progress <70> percent");
 	sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
+	mavlink_log_info(mavlink_fd, "mag cal progress <80> percent");
 
 	free(x);
 	free(y);
@@ -269,6 +246,7 @@ int do_mag_calibration(int mavlink_fd)
 		if (save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
 			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
+			close(sub_mag);
 			return ERROR;
 		}
 
@@ -288,11 +266,13 @@ int do_mag_calibration(int mavlink_fd)
 		mavlink_log_info(mavlink_fd, "magnetometer calibration completed");
 		mavlink_log_info(mavlink_fd, "mag cal progress <100> percent");
 
+		close(sub_mag);
 		return OK;
 		/* third beep by cal end routine */
 
 	} else {
 		mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)");
+		close(sub_mag);
 		return ERROR;
 	}
 }
diff --git a/src/modules/commander/rc_calibration.cpp b/src/modules/commander/rc_calibration.cpp
index fe87a3323..90ede499a 100644
--- a/src/modules/commander/rc_calibration.cpp
+++ b/src/modules/commander/rc_calibration.cpp
@@ -40,6 +40,7 @@
 #include "commander_helper.h"
 
 #include <poll.h>
+#include <unistd.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <mavlink/mavlink_log.h>
@@ -56,14 +57,16 @@ int do_rc_calibration(int mavlink_fd)
 {
 	mavlink_log_info(mavlink_fd, "trim calibration starting");
 
-	/* XXX fix this */
-	// if (current_status.rc_signal) {
-	// 	mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal.");
-	// 	return;
-	// }
-
 	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
 	struct manual_control_setpoint_s sp;
+	bool changed;
+	orb_check(sub_man, &changed);
+
+	if (!changed) {
+		mavlink_log_critical(mavlink_fd, "no manual control, aborting");
+		return ERROR;
+	}
+
 	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
 
 	/* set parameters */
@@ -80,9 +83,11 @@ int do_rc_calibration(int mavlink_fd)
 
 	if (save_ret != 0) {
 		mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed");
+		close(sub_man);
 		return ERROR;
 	}
 
 	mavlink_log_info(mavlink_fd, "trim calibration done");
+	close(sub_man);
 	return OK;
 }
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 674f3feda..3cef10185 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -348,20 +348,15 @@ navigation_state_transition(struct vehicle_status_s *status, navigation_state_t
 			break;
 
 		case NAVIGATION_STATE_AUTO_TAKEOFF:
-
-			/* only transitions from AUTO_READY */
-			if (status->navigation_state == NAVIGATION_STATE_AUTO_READY) {
-				ret = TRANSITION_CHANGED;
-				control_mode->flag_control_rates_enabled = true;
-				control_mode->flag_control_attitude_enabled = true;
-				control_mode->flag_control_velocity_enabled = true;
-				control_mode->flag_control_position_enabled = true;
-				control_mode->flag_control_altitude_enabled = true;
-				control_mode->flag_control_climb_rate_enabled = true;
-				control_mode->flag_control_manual_enabled = false;
-				control_mode->flag_control_auto_enabled = true;
-			}
-
+			ret = TRANSITION_CHANGED;
+			control_mode->flag_control_rates_enabled = true;
+			control_mode->flag_control_attitude_enabled = true;
+			control_mode->flag_control_velocity_enabled = true;
+			control_mode->flag_control_position_enabled = true;
+			control_mode->flag_control_altitude_enabled = true;
+			control_mode->flag_control_climb_rate_enabled = true;
+			control_mode->flag_control_manual_enabled = false;
+			control_mode->flag_control_auto_enabled = true;
 			break;
 
 		case NAVIGATION_STATE_AUTO_LOITER:
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index d7b0fa9c7..5eb7cba9b 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -516,7 +516,7 @@ void mavlink_update_system(void)
 int mavlink_thread_main(int argc, char *argv[])
 {
 	/* initialize mavlink text message buffering */
-	mavlink_logbuffer_init(&lb, 5);
+	mavlink_logbuffer_init(&lb, 10);
 
 	int ch;
 	char *device_name = "/dev/ttyS1";
@@ -738,6 +738,9 @@ int mavlink_thread_main(int argc, char *argv[])
 	/* Reset the UART flags to original state */
 	tcsetattr(uart, TCSANOW, &uart_config_original);
 
+	/* destroy log buffer */
+	//mavlink_logbuffer_destroy(&lb);
+
 	thread_running = false;
 
 	exit(0);
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index af43542da..4674f7a24 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -747,6 +747,7 @@ receive_start(int uart)
 	fcntl(uart, F_SETFL, flags | O_NONBLOCK);
 
 	struct sched_param param;
+	(void)pthread_attr_getschedparam(&receiveloop_attr, &param);
 	param.sched_priority = SCHED_PRIORITY_MAX - 40;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);
 
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control.c b/src/modules/multirotor_pos_control/multirotor_pos_control.c
index 8227f76c5..336523072 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control.c
@@ -227,7 +227,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	hrt_abstime t_prev = 0;
 	const float alt_ctl_dz = 0.2f;
 	const float pos_ctl_dz = 0.05f;
-	const float takeoff_alt_default = 10.0f;
+
 	float ref_alt = 0.0f;
 	hrt_abstime ref_alt_t = 0;
 	uint64_t local_ref_timestamp = 0;
@@ -244,6 +244,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	parameters_init(&params_h);
 	parameters_update(&params_h, &params);
 
+
 	for (int i = 0; i < 2; i++) {
 		pid_init(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f, PID_MODE_DERIVATIV_SET, 0.02f);
 		pid_init(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, 1.0f, params.tilt_max, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
@@ -252,36 +253,35 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 	pid_init(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max, PID_MODE_DERIVATIV_SET, 0.02f);
 	thrust_pid_init(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min, PID_MODE_DERIVATIV_CALC_NO_SP, 0.02f);
 
-	int paramcheck_counter = 0;
-
 	while (!thread_should_exit) {
-		/* check parameters at 1 Hz */
-		if (++paramcheck_counter >= 50) {
-			paramcheck_counter = 0;
-			bool param_updated;
-			orb_check(param_sub, &param_updated);
 
-			if (param_updated) {
-				parameters_update(&params_h, &params);
+		bool param_updated;
+		orb_check(param_sub, &param_updated);
 
-				for (int i = 0; i < 2; i++) {
-					pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
-					/* use integral_limit_out = tilt_max / 2 */
-					float i_limit;
+		if (param_updated) {
+			/* clear updated flag */
+			struct parameter_update_s ps;
+			orb_copy(ORB_ID(parameter_update), param_sub, &ps);
+			/* update params */
+			parameters_update(&params_h, &params);
 
-					if (params.xy_vel_i == 0.0) {
-						i_limit = params.tilt_max / params.xy_vel_i / 2.0;
+			for (int i = 0; i < 2; i++) {
+				pid_set_parameters(&(xy_pos_pids[i]), params.xy_p, 0.0f, params.xy_d, 1.0f, 0.0f);
+				/* use integral_limit_out = tilt_max / 2 */
+				float i_limit;
 
-					} else {
-						i_limit = 1.0f;	// not used really
-					}
+				if (params.xy_vel_i == 0.0f) {
+					i_limit = params.tilt_max / params.xy_vel_i / 2.0f;
 
-					pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
+				} else {
+					i_limit = 1.0f;	// not used really
 				}
 
-				pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
-				thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
+				pid_set_parameters(&(xy_vel_pids[i]), params.xy_vel_p, params.xy_vel_i, params.xy_vel_d, i_limit, params.tilt_max);
 			}
+
+			pid_set_parameters(&z_pos_pid, params.z_p, 0.0f, params.z_d, 1.0f, params.z_vel_max);
+			thrust_pid_set_parameters(&z_vel_pid, params.z_vel_p, params.z_vel_i, params.z_vel_d, -params.thr_max, -params.thr_min);
 		}
 
 		bool updated;
@@ -351,7 +351,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					if (reset_sp_z) {
 						reset_sp_z = false;
 						local_pos_sp.z = local_pos.z;
-						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", -local_pos_sp.z);
+						mavlink_log_info(mavlink_fd, "[mpc] reset alt sp: %.2f", (double) - local_pos_sp.z);
 					}
 
 					/* move altitude setpoint with throttle stick */
@@ -377,7 +377,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						local_pos_sp.y = local_pos.y;
 						pid_reset_integral(&xy_vel_pids[0]);
 						pid_reset_integral(&xy_vel_pids[1]);
-						mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
+						mavlink_log_info(mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
 					}
 
 					/* move position setpoint with roll/pitch stick */
@@ -424,12 +424,12 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					if (reset_auto_pos) {
 						local_pos_sp.x = local_pos.x;
 						local_pos_sp.y = local_pos.y;
-						local_pos_sp.z = -takeoff_alt_default;
+						local_pos_sp.z = - params.takeoff_alt - params.takeoff_gap;
 						local_pos_sp.yaw = att.yaw;
 						local_pos_sp_valid = true;
 						att_sp.yaw_body = att.yaw;
 						reset_auto_pos = false;
-						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", local_pos_sp.x, local_pos_sp.y, -local_pos_sp.z);
+						mavlink_log_info(mavlink_fd, "[mpc] takeoff sp: %.2f %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y, (double) - local_pos_sp.z);
 					}
 
 				} else if (control_mode.auto_state == NAVIGATION_STATE_AUTO_RTL) {
@@ -444,7 +444,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						double lat_home = local_pos.ref_lat * 1e-7;
 						double lon_home = local_pos.ref_lon * 1e-7;
 						map_projection_init(lat_home, lon_home);
-						mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", lat_home, lon_home);
+						mavlink_log_info(mavlink_fd, "[mpc] local pos ref: %.7f, %.7f", (double)lat_home, (double)lon_home);
 					}
 
 					if (global_pos_sp_reproject) {
@@ -468,7 +468,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 							local_pos_sp.yaw = global_pos_sp.yaw;
 							att_sp.yaw_body = global_pos_sp.yaw;
 
-							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", sp_lat, sp_lon, local_pos_sp.x, local_pos_sp.y);
+							mavlink_log_info(mavlink_fd, "[mpc] new sp: %.7f, %.7f (%.2f, %.2f)", (double)sp_lat, sp_lon, (double)local_pos_sp.x, (double)local_pos_sp.y);
 
 						} else {
 							if (!local_pos_sp_valid) {
@@ -481,7 +481,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 								local_pos_sp_valid = true;
 							}
 
-							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", local_pos_sp.x, local_pos_sp.y);
+							mavlink_log_info(mavlink_fd, "[mpc] no global pos sp, loiter: %.2f, %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
 						}
 					}
 
@@ -505,7 +505,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						/* set altitude setpoint to 5m under ground,
 						 * don't set it too deep to avoid unexpected landing in case of false "landed" signal */
 						local_pos_sp.z = 5.0f;
-						mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", -local_pos_sp.z);
+						mavlink_log_info(mavlink_fd, "[mpc] landed, set alt: %.2f", (double) - local_pos_sp.z);
 					}
 
 					reset_sp_z = true;
@@ -515,7 +515,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 					if (reset_sp_z) {
 						reset_sp_z = false;
 						local_pos_sp.z = local_pos.z;
-						mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", -local_pos_sp.z);
+						mavlink_log_info(mavlink_fd, "[mpc] set loiter alt: %.2f", (double) - local_pos_sp.z);
 					}
 				}
 
@@ -527,7 +527,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						local_pos_sp.yaw = att.yaw;
 						local_pos_sp_valid = true;
 						att_sp.yaw_body = att.yaw;
-						mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", local_pos_sp.x, local_pos_sp.y);
+						mavlink_log_info(mavlink_fd, "[mpc] set loiter pos: %.2f %.2f", (double)local_pos_sp.x, (double)local_pos_sp.y);
 					}
 				}
 			}
@@ -588,7 +588,7 @@ static int multirotor_pos_control_thread_main(int argc, char *argv[])
 						}
 
 						thrust_pid_set_integral(&z_vel_pid, -i);
-						mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", i);
+						mavlink_log_info(mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
 					}
 
 					thrust_sp[2] = thrust_pid_calculate(&z_vel_pid, global_vel_sp.vz, local_pos.vz, dt, att.R[2][2]);
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
index 9c1ef2edb..bf9578ea3 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.c
@@ -59,6 +59,8 @@ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 0.5f);
 
 int parameters_init(struct multirotor_position_control_param_handles *h)
 {
+	h->takeoff_alt = param_find("NAV_TAKEOFF_ALT");
+	h->takeoff_gap = param_find("NAV_TAKEOFF_GAP");
 	h->thr_min 	=	param_find("MPC_THR_MIN");
 	h->thr_max 	=	param_find("MPC_THR_MAX");
 	h->z_p 	=	param_find("MPC_Z_P");
@@ -84,6 +86,8 @@ int parameters_init(struct multirotor_position_control_param_handles *h)
 
 int parameters_update(const struct multirotor_position_control_param_handles *h, struct multirotor_position_control_params *p)
 {
+	param_get(h->takeoff_alt, &(p->takeoff_alt));
+	param_get(h->takeoff_gap, &(p->takeoff_gap));
 	param_get(h->thr_min, &(p->thr_min));
 	param_get(h->thr_max, &(p->thr_max));
 	param_get(h->z_p, &(p->z_p));
diff --git a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
index 3ec85a364..fc658dadb 100644
--- a/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
+++ b/src/modules/multirotor_pos_control/multirotor_pos_control_params.h
@@ -41,6 +41,8 @@
 #include <systemlib/param/param.h>
 
 struct multirotor_position_control_params {
+	float takeoff_alt;
+	float takeoff_gap;
 	float thr_min;
 	float thr_max;
 	float z_p;
@@ -63,6 +65,8 @@ struct multirotor_position_control_params {
 };
 
 struct multirotor_position_control_param_handles {
+	param_t takeoff_alt;
+	param_t takeoff_gap;
 	param_t thr_min;
 	param_t thr_max;
 	param_t z_p;
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 4a4572b09..932f61088 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -53,7 +53,7 @@
 #include <math.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
-#include <uORB/topics/actuator_controls_effective.h>
+#include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -181,7 +181,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	uint32_t baro_counter = 0;
 
 	/* declare and safely initialize all structs */
-	struct actuator_controls_effective_s actuator;
+	struct actuator_controls_s actuator;
 	memset(&actuator, 0, sizeof(actuator));
 	struct actuator_armed_s armed;
 	memset(&armed, 0, sizeof(armed));
@@ -200,7 +200,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 	/* subscribe */
 	int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update));
-	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE);
+	int actuator_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS);
 	int armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 	int sensor_combined_sub = orb_subscribe(ORB_ID(sensor_combined));
 	int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -337,7 +337,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			/* actuator */
 			if (fds[1].revents & POLLIN) {
-				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS_EFFECTIVE, actuator_sub, &actuator);
+				orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_sub, &actuator);
 			}
 
 			/* armed */
@@ -429,26 +429,31 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			if (fds[6].revents & POLLIN) {
 				orb_copy(ORB_ID(vehicle_gps_position), vehicle_gps_position_sub, &gps);
 
-				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout && gps.eph_m < 4.0f) {
+				if (gps.fix_type >= 3 && t < gps.timestamp_position + gps_timeout) {
 					/* initialize reference position if needed */
 					if (!ref_xy_inited) {
-						if (ref_xy_init_start == 0) {
-							ref_xy_init_start = t;
-
-						} else if (t > ref_xy_init_start + ref_xy_init_delay) {
-							ref_xy_inited = true;
-							/* reference GPS position */
-							double lat = gps.lat * 1e-7;
-							double lon = gps.lon * 1e-7;
-
-							local_pos.ref_lat = gps.lat;
-							local_pos.ref_lon = gps.lon;
-							local_pos.ref_timestamp = t;
-
-							/* initialize projection */
-							map_projection_init(lat, lon);
-							warnx("init GPS: lat = %.10f,  lon = %.10f", lat, lon);
-							mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
+						/* require EPH < 10m */
+						if (gps.eph_m < 10.0f) {
+							if (ref_xy_init_start == 0) {
+								ref_xy_init_start = t;
+
+							} else if (t > ref_xy_init_start + ref_xy_init_delay) {
+								ref_xy_inited = true;
+								/* reference GPS position */
+								double lat = gps.lat * 1e-7;
+								double lon = gps.lon * 1e-7;
+
+								local_pos.ref_lat = gps.lat;
+								local_pos.ref_lon = gps.lon;
+								local_pos.ref_timestamp = t;
+
+								/* initialize projection */
+								map_projection_init(lat, lon);
+								warnx("init GPS: lat = %.10f,  lon = %.10f", lat, lon);
+								mavlink_log_info(mavlink_fd, "[inav] init GPS: %.7f, %.7f", lat, lon);
+							}
+						} else {
+							ref_xy_init_start = 0;
 						}
 					}
 
@@ -541,7 +546,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		alt_disp = alt_disp * alt_disp;
 		float land_disp2 = params.land_disp * params.land_disp;
 		/* get actual thrust output */
-		float thrust = armed.armed ? actuator.control_effective[3] : 0.0f;
+		float thrust = armed.armed ? actuator.control[3] : 0.0f;
 
 		if (landed) {
 			if (alt_disp > land_disp2 && thrust > params.land_thr) {
diff --git a/src/modules/position_estimator_mc/position_estimator_mc_main.c b/src/modules/position_estimator_mc/position_estimator_mc_main.c
index 984bd1329..363961819 100755
--- a/src/modules/position_estimator_mc/position_estimator_mc_main.c
+++ b/src/modules/position_estimator_mc/position_estimator_mc_main.c
@@ -497,7 +497,12 @@ int position_estimator_mc_thread_main(int argc, char *argv[])
 				local_pos_est.vz = x_z_aposteriori_k[1];
 				local_pos_est.timestamp = hrt_absolute_time();
 				if ((isfinite(x_x_aposteriori_k[0])) && (isfinite(x_x_aposteriori_k[1])) && (isfinite(x_y_aposteriori_k[0])) && (isfinite(x_y_aposteriori_k[1])) && (isfinite(x_z_aposteriori_k[0])) && (isfinite(x_z_aposteriori_k[1]))){
-					orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
+					if(local_pos_est_pub > 0)
+						orb_publish(ORB_ID(vehicle_local_position), local_pos_est_pub, &local_pos_est);
+					else
+						local_pos_est_pub = orb_advertise(ORB_ID(vehicle_local_position), &local_pos_est);
+					//char buf[0xff]; sprintf(buf,"[pos_est_mc] x:%f, y:%f, z:%f",x_x_aposteriori_k[0],x_y_aposteriori_k[0],x_z_aposteriori_k[0]);
+					//mavlink_log_info(mavlink_fd, buf);
 				}
 			}
 		} /* end of poll return value check */
diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp
index deed25836..0edd91b24 100644
--- a/src/modules/px4iofirmware/mixer.cpp
+++ b/src/modules/px4iofirmware/mixer.cpp
@@ -358,8 +358,8 @@ static unsigned mixer_text_length = 0;
 void
 mixer_handle_text(const void *buffer, size_t length)
 {
-	/* do not allow a mixer change while outputs armed */
-	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
+	/* do not allow a mixer change while safety off */
+	if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
 		return;
 	}
 
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 2b21bb5a5..e83fb7dd3 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -890,13 +890,14 @@ int sdlog2_thread_main(int argc, char *argv[])
 			if (fds[ifds++].revents & POLLIN) {
 				// Don't orb_copy, it's already done few lines above
 				log_msg.msg_type = LOG_STAT_MSG;
-				log_msg.body.log_STAT.main_state = (unsigned char) buf_status.main_state;
-				log_msg.body.log_STAT.navigation_state = (unsigned char) buf_status.navigation_state;
-				log_msg.body.log_STAT.arming_state = (unsigned char) buf_status.arming_state;
+				log_msg.body.log_STAT.main_state = (uint8_t) buf_status.main_state;
+				log_msg.body.log_STAT.navigation_state = (uint8_t) buf_status.navigation_state;
+				log_msg.body.log_STAT.arming_state = (uint8_t) buf_status.arming_state;
 				log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage;
 				log_msg.body.log_STAT.battery_current = buf_status.battery_current;
 				log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining;
-				log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning;
+				log_msg.body.log_STAT.battery_warning = (uint8_t) buf_status.battery_warning;
+				log_msg.body.log_STAT.landed = (uint8_t) buf_status.condition_landed;
 				LOGBUFFER_WRITE_AND_COUNT(STAT);
 			}
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index d99892fe2..4eeb65a87 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -155,6 +155,7 @@ struct log_STAT_s {
 	float battery_current;
 	float battery_remaining;
 	uint8_t battery_warning;
+	uint8_t landed;
 };
 
 /* --- RC - RC INPUT CHANNELS --- */
@@ -263,7 +264,7 @@ static const struct log_format_s log_formats[] = {
 	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(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),
-	LOG_FORMAT(STAT, "BBBfffB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn"),
+	LOG_FORMAT(STAT, "BBBfffBB", "MainState,NavState,ArmState,BatV,BatC,BatRem,BatWarn,Landed"),
 	LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"),
 	LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"),
 	LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"),
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index fd2a6318e..992abf2cc 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -68,7 +68,8 @@ PARAM_DEFINE_FLOAT(SENS_ACC_XSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_YSCALE, 1.0f);
 PARAM_DEFINE_FLOAT(SENS_ACC_ZSCALE, 1.0f);
 
-PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0);
+PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
+PARAM_DEFINE_INT32(SENS_DPRES_ANA, 0);
 
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
 PARAM_DEFINE_INT32(SENS_EXT_MAG_ROT, 0);
@@ -157,6 +158,13 @@ PARAM_DEFINE_FLOAT(RC14_MAX, 2000);
 PARAM_DEFINE_FLOAT(RC14_REV, 1.0f);
 PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f);
 
+PARAM_DEFINE_FLOAT(RC15_MIN, 1000);
+PARAM_DEFINE_FLOAT(RC15_TRIM, 1500);
+PARAM_DEFINE_FLOAT(RC15_MAX, 2000);
+PARAM_DEFINE_FLOAT(RC15_REV, 1.0f);
+PARAM_DEFINE_FLOAT(RC15_DZ, 0.0f);
+
+
 PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */
 PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */
 
@@ -164,6 +172,8 @@ PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
 #else
 /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */
+/* PX4IOAR: 0.00838095238 */
+/* FMU standalone: 1/(10 / (47+10)) * (3.3 / 4095) = 0.00459340659 */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f));
 #endif
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index e857b1c2f..e98c4d548 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -301,6 +301,7 @@ private:
 		float accel_offset[3];
 		float accel_scale[3];
 		float diff_pres_offset_pa;
+		float diff_pres_analog_enabled;
 
 		int board_rotation;
 		int external_mag_rotation;
@@ -348,6 +349,7 @@ private:
 		param_t mag_offset[3];
 		param_t mag_scale[3];
 		param_t diff_pres_offset_pa;
+		param_t diff_pres_analog_enabled;
 
 		param_t rc_map_roll;
 		param_t rc_map_pitch;
@@ -617,6 +619,7 @@ Sensors::Sensors() :
 
 	/* Differential pressure offset */
 	_parameter_handles.diff_pres_offset_pa = param_find("SENS_DPRES_OFF");
+	_parameter_handles.diff_pres_analog_enabled = param_find("SENS_DPRES_ANA");
 
 	_parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING");
 
@@ -657,7 +660,9 @@ int
 Sensors::parameters_update()
 {
 	bool rc_valid = true;
-
+    float tmpScaleFactor = 0.0f;
+    float tmpRevFactor = 0.0f;
+    
 	/* rc values */
 	for (unsigned int i = 0; i < RC_CHANNELS_MAX; i++) {
 
@@ -667,18 +672,21 @@ Sensors::parameters_update()
 		param_get(_parameter_handles.rev[i], &(_parameters.rev[i]));
 		param_get(_parameter_handles.dz[i], &(_parameters.dz[i]));
 
-		_parameters.scaling_factor[i] = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
-
+		tmpScaleFactor = (1.0f / ((_parameters.max[i] - _parameters.min[i]) / 2.0f) * _parameters.rev[i]);
+		tmpRevFactor = tmpScaleFactor * _parameters.rev[i];
+        
 		/* handle blowup in the scaling factor calculation */
-		if (!isfinite(_parameters.scaling_factor[i]) ||
-		    _parameters.scaling_factor[i] * _parameters.rev[i] < 0.000001f ||
-		    _parameters.scaling_factor[i] * _parameters.rev[i] > 0.2f) {
-
+		if (!isfinite(tmpScaleFactor) ||
+		    (tmpRevFactor < 0.000001f) ||
+		    (tmpRevFactor > 0.2f) ) {
+			warnx("RC chan %u not sane, scaling: %8.6f, rev: %d", i, tmpScaleFactor, (int)(_parameters.rev[i]));
 			/* scaling factors do not make sense, lock them down */
-			_parameters.scaling_factor[i] = 0;
+			_parameters.scaling_factor[i] = 0.0f;
 			rc_valid = false;
 		}
-
+        else {
+            _parameters.scaling_factor[i] = tmpScaleFactor;
+        }
 	}
 
 	/* handle wrong values */
@@ -784,6 +792,7 @@ Sensors::parameters_update()
 
 	/* Airspeed offset */
 	param_get(_parameter_handles.diff_pres_offset_pa, &(_parameters.diff_pres_offset_pa));
+	param_get(_parameter_handles.diff_pres_analog_enabled, &(_parameters.diff_pres_analog_enabled));
 
 	/* scaling of ADC ticks to battery voltage */
 	if (param_get(_parameter_handles.battery_voltage_scaling, &(_parameters.battery_voltage_scaling)) != OK) {
@@ -1266,9 +1275,10 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 
 					/**
 					 * The voltage divider pulls the signal down, only act on
-					 * a valid voltage from a connected sensor
+					 * a valid voltage from a connected sensor. Also assume a non-
+					 * zero offset from the sensor if its connected.
 					 */
-					if (voltage > 0.4f) {
+					if (voltage > 0.4f && _parameters.diff_pres_analog_enabled) {
 
 						float diff_pres_pa = voltage * 1000.0f - _parameters.diff_pres_offset_pa; //for MPXV7002DP sensor
 
diff --git a/src/modules/systemlib/mavlink_log.c b/src/modules/systemlib/mavlink_log.c
index 27608bdbf..f321f9ceb 100644
--- a/src/modules/systemlib/mavlink_log.c
+++ b/src/modules/systemlib/mavlink_log.c
@@ -51,7 +51,15 @@ __EXPORT void mavlink_logbuffer_init(struct mavlink_logbuffer *lb, int size)
 	lb->size  = size;
 	lb->start = 0;
 	lb->count = 0;
-	lb->elems = (struct mavlink_logmessage *)calloc(lb->size, sizeof(struct mavlink_logmessage));
+	lb->elems = calloc(lb->size, sizeof(struct mavlink_logmessage));
+}
+
+__EXPORT void mavlink_logbuffer_destroy(struct mavlink_logbuffer *lb)
+{
+	lb->size  = 0;
+	lb->start = 0;
+	lb->count = 0;
+	free(lb->elems);
 }
 
 __EXPORT int mavlink_logbuffer_is_full(struct mavlink_logbuffer *lb)
diff --git a/src/modules/systemlib/param/param.c b/src/modules/systemlib/param/param.c
index 69a9bdf9b..c69de52b7 100644
--- a/src/modules/systemlib/param/param.c
+++ b/src/modules/systemlib/param/param.c
@@ -505,24 +505,31 @@ param_get_default_file(void)
 int
 param_save_default(void)
 {
+	int result;
+
 	/* delete the file in case it exists */
-	unlink(param_get_default_file());
+	struct stat buffer;
+	if (stat(param_get_default_file(), &buffer) == 0) {
+		result = unlink(param_get_default_file());
+		if (result != OK)
+			warnx("unlinking file %s failed.", param_get_default_file());
+	}
 
 	/* create the file */
 	int fd = open(param_get_default_file(), O_WRONLY | O_CREAT | O_EXCL);
 
 	if (fd < 0) {
 		warn("opening '%s' for writing failed", param_get_default_file());
-		return -1;
+		return fd;
 	}
 
-	int result = param_export(fd, false);
+	result = param_export(fd, false);
 	close(fd);
 
 	if (result != 0) {
 		warn("error exporting parameters to '%s'", param_get_default_file());
 		unlink(param_get_default_file());
-		return -2;
+		return result;
 	}
 
 	return 0;
diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c
index 4996a8f66..739503ed4 100644
--- a/src/modules/systemlib/pid/pid.c
+++ b/src/modules/systemlib/pid/pid.c
@@ -51,6 +51,8 @@
 #include "pid.h"
 #include <math.h>
 
+#define SIGMA 0.000001f
+
 __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax,
 		       float limit, uint8_t mode, float dt_min)
 {
@@ -168,8 +170,8 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	// Calculate the error integral and check for saturation
 	i = pid->integral + (error * dt);
 
-	if (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit ||
-	    fabsf(i) > pid->intmax) {
+	if ((pid->limit > SIGMA && (fabsf((error * pid->kp) + (i * pid->ki) + (d * pid->kd)) > pid->limit)) ||
+			fabsf(i) > pid->intmax) {
 		i = pid->integral;		// If saturated then do not update integral value
 		pid->saturated = 1;
 
@@ -186,11 +188,13 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo
 	float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd);
 
 	if (isfinite(output)) {
-		if (output > pid->limit) {
-			output = pid->limit;
+		if (pid->limit > SIGMA) {
+			if (output > pid->limit) {
+				output = pid->limit;
 
-		} else if (output < -pid->limit) {
-			output = -pid->limit;
+			} else if (output < -pid->limit) {
+				output = -pid->limit;
+			}
 		}
 
 		pid->last_output = output;
diff --git a/src/modules/systemlib/rc_check.c b/src/modules/systemlib/rc_check.c
index 9d47d8000..60d6473b8 100644
--- a/src/modules/systemlib/rc_check.c
+++ b/src/modules/systemlib/rc_check.c
@@ -66,7 +66,7 @@ int rc_calibration_check(void) {
 		// 	count++;
 		// }
 
-
+	int channel_fail_count = 0;
 
 	for (int i = 0; i < RC_CHANNELS_MAX; i++) {
 		/* should the channel be enabled? */
@@ -112,13 +112,13 @@ int rc_calibration_check(void) {
 		}
 		if (param_trim < param_min) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN", i+1);
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM < MIN (%d/%d)", i+1, (int)param_trim, (int)param_min);
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
 		if (param_trim > param_max) {
 			count++;
-			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX", i+1);
+			mavlink_log_critical(mavlink_fd, "ERR: RC_%d_TRIM > MAX (%d/%d)", i+1, (int)param_trim, (int)param_max);
 			/* give system time to flush error message in case there are more */
 			usleep(100000);
 		}
@@ -142,7 +142,12 @@ int rc_calibration_check(void) {
 		/* sanity checks pass, enable channel */
 		if (count) {
 			mavlink_log_critical(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
+			warnx(mavlink_fd, "ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
 			usleep(100000);
 		}
+
+		channel_fail_count += count;
 	}
+
+	return channel_fail_count;
 }
diff --git a/src/modules/uORB/topics/vehicle_attitude_setpoint.h b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
index a7cda34a8..686fd765c 100644
--- a/src/modules/uORB/topics/vehicle_attitude_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_attitude_setpoint.h
@@ -64,6 +64,12 @@ struct vehicle_attitude_setpoint_s
 	float R_body[9];				/**< Rotation matrix describing the setpoint as rotation from the current body frame */
 	bool R_valid;					/**< Set to true if rotation matrix is valid */
 
+	//! For quaternion-based attitude control
+	float q_d[4];				/** Desired quaternion for quaternion control */
+	bool q_d_valid;					/**< Set to true if quaternion vector is valid */
+	float q_e[4];				/** Attitude error in quaternion */
+	bool q_e_valid;					/**< Set to true if quaternion error vector is valid */
+
 	float thrust;					/**< Thrust in Newton the power system should generate */
 
 };
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 43218eac4..1c184d3a7 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -153,9 +153,9 @@ enum VEHICLE_TYPE {
 };
 
 enum VEHICLE_BATTERY_WARNING {
-    VEHICLE_BATTERY_WARNING_NONE = 0,    /**< no battery low voltage warning active */
-    VEHICLE_BATTERY_WARNING_WARNING,        /**< warning of low voltage 1. stage */
-    VEHICLE_BATTERY_WARNING_ALERT            /**< alerting of low voltage 2. stage */
+    VEHICLE_BATTERY_WARNING_NONE = 0,	/**< no battery low voltage warning active */
+    VEHICLE_BATTERY_WARNING_LOW,	/**< warning of low voltage */
+    VEHICLE_BATTERY_WARNING_CRITICAL	/**< alerting of critical voltage */
 };
 
 /**