Merge branch 'master' into navigator_rewrite

Conflicts: src/drivers/gps/gps.cpp src/drivers/gps/mtk.cpp src/modules/commander/commander.cpp src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp src/modules/navigator/mission.cpp src/modules/navigator/mission.h src/modules/navigator/navigator_main.cpp src/modules/navigator/navigator_state.h src/modules/position_estimator_inav/position_estimator_inav_main.c
2026-06-27 17:00:35 +08:00 · 2014-05-26 20:19:11 +02:00
parent 68352cb923 36495cdb62
commit 063caba36b
173 changed files with 5312 additions and 1890 deletions
@@ -1,8 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
- *   Author: Tobias Naegeli <naegelit@student.ethz.ch>
- *           Lorenz Meier <lm@inf.ethz.ch>
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -34,9 +32,12 @@
 ****************************************************************************/

 /*
- * @file attitude_estimator_ekf_main.c
+ * @file attitude_estimator_ekf_main.cpp
 *
 * Extended Kalman Filter for Attitude Estimation.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
 */

 #include <nuttx/config.h>
@@ -111,7 +112,7 @@ usage(const char *reason)
 * Makefile does only apply to this management task.
 *
 * The actual stack size should be set in the call
- * to task_create().
+ * to task_spawn_cmd().
 */
 int attitude_estimator_ekf_main(int argc, char *argv[])
 {
@@ -265,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;

@@ -273,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};

@@ -286,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;
@@ -381,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;
 					}

@@ -392,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;
 					}

@@ -444,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;
 					}

@@ -497,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);

@@ -50,3 +50,5 @@ SRCS		 = attitude_estimator_ekf_main.cpp \
 		   codegen/rtGetNaN.c \
 		   codegen/norm.c \
 		   codegen/cross.c
+
+MODULE_STACKSIZE = 1200
@@ -1,8 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (C) 2013 PX4 Development Team. All rights reserved.
- *   Author: Hyon Lim <limhyon@gmail.com>
- *           Anton Babushkin <anton.babushkin@me.com>
+ *   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
@@ -36,6 +34,9 @@
 /*
 * @file attitude_estimator_so3_main.cpp
 *
+ * @author Hyon Lim <limhyon@gmail.com>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
 * Implementation of nonlinear complementary filters on the SO(3).
 * This code performs attitude estimation by using accelerometer, gyroscopes and magnetometer.
 * Result is provided as quaternion, 1-2-3 Euler angle and rotation matrix.
@@ -131,7 +132,7 @@ usage(const char *reason)
 * Makefile does only apply to this management task.
 *
 * The actual stack size should be set in the call
- * to task_create().
+ * to task_spawn_cmd().
 */
 int attitude_estimator_so3_main(int argc, char *argv[])
 {
@@ -6,3 +6,5 @@ MODULE_COMMAND	 = attitude_estimator_so3

 SRCS		 = attitude_estimator_so3_main.cpp \
 		   attitude_estimator_so3_params.c
+
+MODULE_STACKSIZE = 1200
@@ -194,13 +194,13 @@ int do_accel_calibration(int mavlink_fd)
 		int32_t board_rotation_int;
 		param_get(board_rotation_h, &(board_rotation_int));
 		enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
-		math::Matrix<3,3> board_rotation;
+		math::Matrix<3, 3> board_rotation;
 		get_rot_matrix(board_rotation_id, &board_rotation);
-		math::Matrix<3,3> board_rotation_t = board_rotation.transposed();
+		math::Matrix<3, 3> board_rotation_t = board_rotation.transposed();
 		math::Vector<3> accel_offs_vec(&accel_offs[0]);
-		math::Vector<3> accel_offs_rotated = board_rotation_t * accel_offs_vec;
-		math::Matrix<3,3> accel_T_mat(&accel_T[0][0]);
-		math::Matrix<3,3> accel_T_rotated = board_rotation_t * accel_T_mat * board_rotation;
+		math::Vector<3> accel_offs_rotated = board_rotation_t *accel_offs_vec;
+		math::Matrix<3, 3> accel_T_mat(&accel_T[0][0]);
+		math::Matrix<3, 3> accel_T_rotated = board_rotation_t *accel_T_mat * board_rotation;

 		accel_scale.x_offset = accel_offs_rotated(0);
 		accel_scale.x_scale = accel_T_rotated(0, 0);
@@ -277,11 +277,13 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 			}
 		}

-		if (old_done_count != done_count)
+		if (old_done_count != done_count) {
 			mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 17 * done_count);
+		}

-		if (done)
+		if (done) {
 			break;
+		}

 		mavlink_log_info(mavlink_fd, "directions left: %s%s%s%s%s%s",
 				 (!data_collected[0]) ? "x+ " : "",
@@ -380,11 +382,13 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)
 				d = d * d;
 				accel_disp[i] = accel_disp[i] * (1.0f - w);

-				if (d > still_thr2 * 8.0f)
+				if (d > still_thr2 * 8.0f) {
 					d = still_thr2 * 8.0f;
+				}

-				if (d > accel_disp[i])
+				if (d > accel_disp[i]) {
 					accel_disp[i] = d;
+				}
 			}

 			/* still detector with hysteresis */
@@ -432,33 +436,39 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined)

 	if (fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr &&
 	    fabsf(accel_ema[1]) < accel_err_thr &&
-	    fabsf(accel_ema[2]) < accel_err_thr)
-		return 0;	// [ g, 0, 0 ]
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return 0;        // [ g, 0, 0 ]
+	}

 	if (fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr &&
 	    fabsf(accel_ema[1]) < accel_err_thr &&
-	    fabsf(accel_ema[2]) < accel_err_thr)
-		return 1;	// [ -g, 0, 0 ]
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return 1;        // [ -g, 0, 0 ]
+	}

 	if (fabsf(accel_ema[0]) < accel_err_thr &&
 	    fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr &&
-	    fabsf(accel_ema[2]) < accel_err_thr)
-		return 2;	// [ 0, g, 0 ]
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return 2;        // [ 0, g, 0 ]
+	}

 	if (fabsf(accel_ema[0]) < accel_err_thr &&
 	    fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr &&
-	    fabsf(accel_ema[2]) < accel_err_thr)
-		return 3;	// [ 0, -g, 0 ]
+	    fabsf(accel_ema[2]) < accel_err_thr) {
+		return 3;        // [ 0, -g, 0 ]
+	}

 	if (fabsf(accel_ema[0]) < accel_err_thr &&
 	    fabsf(accel_ema[1]) < accel_err_thr &&
-	    fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr)
-		return 4;	// [ 0, 0, g ]
+	    fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr) {
+		return 4;        // [ 0, 0, g ]
+	}

 	if (fabsf(accel_ema[0]) < accel_err_thr &&
 	    fabsf(accel_ema[1]) < accel_err_thr &&
-	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr)
-		return 5;	// [ 0, 0, -g ]
+	    fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr) {
+		return 5;        // [ 0, 0, -g ]
+	}

 	mavlink_log_critical(mavlink_fd, "ERROR: invalid orientation");

@@ -485,8 +495,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 			struct sensor_combined_s sensor;
 			orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);

-			for (int i = 0; i < 3; i++)
+			for (int i = 0; i < 3; i++) {
 				accel_sum[i] += sensor.accelerometer_m_s2[i];
+			}

 			count++;

@@ -495,8 +506,9 @@ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samp
 			continue;
 		}

-		if (errcount > samples_num / 10)
+		if (errcount > samples_num / 10) {
 			return ERROR;
+		}
 	}

 	for (int i = 0; i < 3; i++) {
@@ -512,8 +524,9 @@ int mat_invert3(float src[3][3], float dst[3][3])
 		    src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) +
 		    src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]);

-	if (det == 0.0f)
-		return ERROR;	// Singular matrix
+	if (det == 0.0f) {
+		return ERROR;        // Singular matrix
+	}

 	dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det;
 	dst[1][0] = (src[1][2] * src[2][0] - src[1][0] * src[2][2]) / det;
@@ -549,8 +562,9 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo
 	/* calculate inverse matrix for A */
 	float mat_A_inv[3][3];

-	if (mat_invert3(mat_A, mat_A_inv) != OK)
+	if (mat_invert3(mat_A, mat_A_inv) != OK) {
 		return ERROR;
+	}

 	/* copy results to accel_T */
 	for (int i = 0; i < 3; i++) {
@@ -82,12 +82,15 @@ int do_airspeed_calibration(int mavlink_fd)

 	bool paramreset_successful = false;
 	int  fd = open(AIRSPEED_DEVICE_PATH, 0);
+
 	if (fd > 0) {
 		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 			paramreset_successful = true;
+
 		} else {
 			mavlink_log_critical(mavlink_fd, "airspeed offset zero failed");
 		}
+
 		close(fd);
 	}

@@ -112,8 +115,9 @@ int do_airspeed_calibration(int mavlink_fd)
 			diff_pres_offset += diff_pres.differential_pressure_raw_pa;
 			calibration_counter++;

-			if (calibration_counter % (calibration_count / 20) == 0)
+			if (calibration_counter % (calibration_count / 20) == 0) {
 				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+			}

 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
@@ -221,7 +221,7 @@ void print_status();

 transition_result_t check_navigation_state_machine(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_local_position_s *local_pos);

-transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy);
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy);

 /**
 * Loop that runs at a lower rate and priority for calibration and parameter tasks.
@@ -233,8 +233,9 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul

 int commander_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		usage("missing command");
+	}

 	if (!strcmp(argv[1], "start")) {

@@ -248,7 +249,7 @@ int commander_main(int argc, char *argv[])
 		daemon_task = task_spawn_cmd("commander",
 					     SCHED_DEFAULT,
 					     SCHED_PRIORITY_MAX - 40,
-					     3000,
+					     2950,
 					     commander_thread_main,
 					     (argv) ? (const char **)&argv[2] : (const char **)NULL);

@@ -261,8 +262,9 @@ int commander_main(int argc, char *argv[])

 	if (!strcmp(argv[1], "stop")) {

-		if (!thread_running)
+		if (!thread_running) {
 			errx(0, "commander already stopped");
+		}

 		thread_should_exit = true;

@@ -304,8 +306,9 @@ int commander_main(int argc, char *argv[])

 void usage(const char *reason)
 {
-	if (reason)
+	if (reason) {
 		fprintf(stderr, "%s\n", reason);
+	}

 	fprintf(stderr, "usage: daemon {start|stop|status} [-p <additional params>]\n\n");
 	exit(1);
@@ -364,20 +367,22 @@ void print_status()

 static orb_advert_t status_pub;

-transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char* armedBy)
+transition_result_t arm_disarm(bool arm, const int mavlink_fd, const char *armedBy)
 {
-    transition_result_t arming_res = TRANSITION_NOT_CHANGED;
-    
-    // Transition the armed state. By passing mavlink_fd to arming_state_transition it will
-    // output appropriate error messages if the state cannot transition.
-    arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
-    if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
-        mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
-    } else if (arming_res == TRANSITION_DENIED) {
-        tune_negative(true);
-    }
-    
-    return arming_res;
+	transition_result_t arming_res = TRANSITION_NOT_CHANGED;
+
+	// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
+	// output appropriate error messages if the state cannot transition.
+	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, mavlink_fd);
+
+	if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
+		mavlink_log_info(mavlink_fd, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
+
+	} else if (arming_res == TRANSITION_DENIED) {
+		tune_negative(true);
+	}
+
+	return arming_res;
 }

 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd, struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
@@ -414,43 +419,61 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		/* set main state */
 		transition_result_t main_res = TRANSITION_DENIED;

-		if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
-			/* use autopilot-specific mode */
-			if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
-				/* MANUAL */
-				main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+		if (hil_ret == OK) {
+			ret = true;
+		}

-			} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) {
-				/* SEATBELT */
-				main_res = main_state_transition(status, MAIN_STATE_SEATBELT);
+		// Transition the arming state
+		arming_res = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");

-			} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) {
-				/* EASY */
-				main_res = main_state_transition(status, MAIN_STATE_EASY);
+		if (arming_res == TRANSITION_CHANGED) {
+			ret = true;
+		}

 			} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) {
 				/* AUTO */
 				main_res = main_state_transition(status, MAIN_STATE_AUTO);
 			}

-		} else {
-			/* use base mode */
-			if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
-				/* AUTO */
-				main_res = main_state_transition(status, MAIN_STATE_AUTO);
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTL) {
+					/* ALTCTL */
+					main_res = main_state_transition(status, MAIN_STATE_ALTCTL);

-			} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
-				if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
-					/* EASY */
-					main_res = main_state_transition(status, MAIN_STATE_EASY);
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTL) {
+					/* POSCTL */
+					main_res = main_state_transition(status, MAIN_STATE_POSCTL);

-				} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
-					/* MANUAL */
-					main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+				} 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 {
+				/* use base mode */
+				if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) {
+					/* AUTO */
+					main_res = main_state_transition(status, MAIN_STATE_AUTO);
+
+				} else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) {
+					if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) {
+						/* POSCTL */
+						main_res = main_state_transition(status, MAIN_STATE_POSCTL);
+
+					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
+						/* MANUAL */
+						main_res = main_state_transition(status, MAIN_STATE_MANUAL);
+					}
 				}
 			}
 		}

+		if (main_res == TRANSITION_CHANGED) {
+			ret = true;
+		}

 		if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) {
 			result = VEHICLE_CMD_RESULT_ACCEPTED;
@@ -463,24 +486,28 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 	}

 	case VEHICLE_CMD_COMPONENT_ARM_DISARM: {
-            // Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
-            // We use an float epsilon delta to test float equality.
-            if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
+			// Follow exactly what the mavlink spec says for values: 0.0f for disarm, 1.0f for arm.
+			// We use an float epsilon delta to test float equality.
+			if (cmd->param1 != 0.0f && (fabsf(cmd->param1 - 1.0f) > 2.0f * FLT_EPSILON)) {
 				mavlink_log_info(mavlink_fd, "Unsupported ARM_DISARM parameter: %.6f", cmd->param1);
-            } else {

-		// Flick to inair restore first if this comes from an onboard system
-		if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
-			status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
-		}
-                transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
-                if (arming_res == TRANSITION_DENIED) {
-                    mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
-                    result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
-                } else {
-                    result = VEHICLE_CMD_RESULT_ACCEPTED;
-                }
-            }
+			} else {
+
+				// Flick to inair restore first if this comes from an onboard system
+				if (cmd->source_system == status->system_id && cmd->source_component == status->component_id) {
+					status->arming_state = ARMING_STATE_IN_AIR_RESTORE;
+				}
+
+				transition_result_t arming_res = arm_disarm(cmd->param1 != 0.0f, mavlink_fd, "arm/disarm component command");
+
+				if (arming_res == TRANSITION_DENIED) {
+					mavlink_log_critical(mavlink_fd, "#audio: REJECTING component arm cmd");
+					result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
+
+				} else {
+					result = VEHICLE_CMD_RESULT_ACCEPTED;
+				}
+			}
 		}
 		break;

@@ -504,7 +531,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		}
 		break;

-		/* Flight termination */
+	/* Flight termination */
 	case VEHICLE_CMD_DO_SET_SERVO: { //xxx: needs its own mavlink command

 			//XXX: to enable the parachute, a param needs to be set
@@ -523,6 +550,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe

 	case VEHICLE_CMD_DO_SET_HOME: {
 			bool use_current = cmd->param1 > 0.5f;
+
 			if (use_current) {
 				/* use current position */
 				if (status->condition_global_position_valid) {
@@ -566,6 +594,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 			}
 		}
 		break;
+
 	case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN:
 	case VEHICLE_CMD_PREFLIGHT_CALIBRATION:
 	case VEHICLE_CMD_PREFLIGHT_SET_SENSOR_OFFSETS:
@@ -579,6 +608,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe
 		answer_command(*cmd, VEHICLE_CMD_RESULT_UNSUPPORTED);
 		break;
 	}
+
 	if (result != VEHICLE_CMD_RESULT_UNSUPPORTED) {
 		/* already warned about unsupported commands in "default" case */
 		answer_command(*cmd, result);
@@ -612,9 +642,10 @@ int commander_thread_main(int argc, char *argv[])

 	char *main_states_str[MAIN_STATE_MAX];
 	main_states_str[0] = "MANUAL";
-	main_states_str[1] = "SEATBELT";
-	main_states_str[2] = "EASY";
+	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";
@@ -705,7 +736,7 @@ 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, 2992);
+	pthread_attr_setstacksize(&commander_low_prio_attr, 2900);

 	struct sched_param param;
 	(void)pthread_attr_getschedparam(&commander_low_prio_attr, &param);
@@ -860,6 +891,7 @@ int commander_thread_main(int argc, char *argv[])
 				/* re-check RC calibration */
 				rc_calibration_ok = (OK == rc_calibration_check(mavlink_fd));
 			}
+
 			/* navigation parameters */
 			param_get(_param_takeoff_alt, &takeoff_alt);
 			param_get(_param_enable_parachute, &parachute_enabled);
@@ -900,6 +932,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* disarm if safety is now on and still armed */
 			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
 				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
+
 				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
 					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
 				}
@@ -925,9 +958,11 @@ int commander_thread_main(int argc, char *argv[])
 		/* update condition_global_position_valid */
 		/* hysteresis for EPH/EPV */
 		bool eph_epv_good;
+
 		if (status.condition_global_position_valid) {
 			if (global_position.eph > eph_epv_threshold * 2.0f || global_position.epv > eph_epv_threshold * 2.0f) {
 				eph_epv_good = false;
+
 			} else {
 				eph_epv_good = true;
 			}
@@ -935,17 +970,19 @@ int commander_thread_main(int argc, char *argv[])
 		} else {
 			if (global_position.eph < eph_epv_threshold && global_position.epv < eph_epv_threshold) {
 				eph_epv_good = true;
+
 			} else {
 				eph_epv_good = false;
 			}
 		}
+
 		check_valid(global_position.timestamp, POSITION_TIMEOUT, eph_epv_good, &(status.condition_global_position_valid), &status_changed);

 		/* check if GPS fix is ok */

 		/* update home position */
 		if (!status.condition_home_position_valid && status.condition_global_position_valid && !armed.armed &&
-			(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
+		    (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {

 			home.lat = global_position.lat;
 			home.lon = global_position.lon;
@@ -972,7 +1009,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);

 		if (status.condition_local_altitude_valid) {
@@ -1050,8 +1106,9 @@ int commander_thread_main(int argc, char *argv[])
 			/* compute system load */
 			uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time;

-			if (last_idle_time > 0)
-				status.load = 1.0f - ((float)interval_runtime / 1e6f);	//system load is time spent in non-idle
+			if (last_idle_time > 0) {
+				status.load = 1.0f - ((float)interval_runtime / 1e6f);        //system load is time spent in non-idle
+			}

 			last_idle_time = system_load.tasks[0].total_runtime;

@@ -1127,22 +1184,22 @@ int commander_thread_main(int argc, char *argv[])

 			status.rc_signal_lost = false;

-			transition_result_t res;	// store all transitions results here
+			transition_result_t arming_res;	// store all transitions results here

 			/* arm/disarm by RC */
-			res = TRANSITION_NOT_CHANGED;
+			arming_res = TRANSITION_NOT_CHANGED;

-			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSISTED mode and landed) -> disarm
+			/* check if left stick is in lower left position and we are in MANUAL or AUTO_READY mode or (ASSIST mode and landed) -> disarm
 			 * 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) &&
-			    sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
+			    (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) {
 					/* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */
 					arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-					res = arming_state_transition(&status, &safety, new_arming_state, &armed);
+					arming_res = arming_state_transition(&status, &safety, new_arming_state, &armed);
 					stick_off_counter = 0;

 				} else {
@@ -1155,16 +1212,16 @@ int commander_thread_main(int argc, char *argv[])

 			/* check if left stick is in lower right position and we're in MANUAL mode -> arm */
 			if (status.arming_state == ARMING_STATE_STANDBY &&
-			    sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < 0.1f) {
+			    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 {
-						res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
+						arming_res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed);
 					}

 					stick_on_counter = 0;
@@ -1177,7 +1234,7 @@ int commander_thread_main(int argc, char *argv[])
 				stick_on_counter = 0;
 			}

-			if (res == TRANSITION_CHANGED) {
+			if (arming_res == TRANSITION_CHANGED) {
 				if (status.arming_state == ARMING_STATE_ARMED) {
 					mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC");

@@ -1185,24 +1242,24 @@ int commander_thread_main(int argc, char *argv[])
 					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC");
 				}

-			} else if (res == TRANSITION_DENIED) {
+			} else if (arming_res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
 				mavlink_log_critical(mavlink_fd, "ERROR: arming state transition denied");
 			}

 			if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
 				/* recover from failsafe */
-				transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+				(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
 			}

 			/* evaluate the main state machine according to mode switches */
-			res = set_main_state_rc(&status, &sp_man);
+			transition_result_t main_res = set_main_state_rc(&status, &sp_man);

 			/* play tune on mode change only if armed, blink LED always */
-			if (res == TRANSITION_CHANGED) {
+			if (main_res == TRANSITION_CHANGED) {
 				tune_positive(armed.armed);

-			} else if (res == TRANSITION_DENIED) {
+			} else if (main_res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
 				mavlink_log_critical(mavlink_fd, "ERROR: main state transition denied");
 			}
@@ -1215,7 +1272,8 @@ int commander_thread_main(int argc, char *argv[])


 			} else {
-				/* MISSION switch */
+
+				/* LOITER switch */
 				if (sp_man.loiter_switch == SWITCH_POS_ON) {
 					/* stick is in LOITER position */
 					status.set_nav_state = NAVIGATION_STATE_LOITER;
@@ -1225,7 +1283,7 @@ int commander_thread_main(int argc, char *argv[])
 					status.set_nav_state = NAVIGATION_STATE_MISSION;

 				} else if ((sp_man.return_switch == SWITCH_POS_OFF || sp_man.return_switch == SWITCH_POS_MIDDLE) &&
-						pos_sp_triplet.nav_state == NAV_STATE_RTL) {
+					   pos_sp_triplet.nav_state == NAV_STATE_RTL) {
 					/* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */
 					status.set_nav_state = NAVIGATION_STATE_MISSION;
 				}
@@ -1241,27 +1299,20 @@ int commander_thread_main(int argc, char *argv[])
 			if (armed.armed) {
 				if (status.main_state == MAIN_STATE_AUTO) {
 					/* check if AUTO mode still allowed */
-					transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO);
+					transition_result_t auto_res = main_state_transition(&status, MAIN_STATE_AUTO);

-					if (res == TRANSITION_NOT_CHANGED) {
+					if (auto_res == TRANSITION_NOT_CHANGED) {
 						last_auto_state_valid = hrt_absolute_time();
 					}

 					/* still invalid state after the timeout interval, execute failsafe */
-					if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (res == TRANSITION_DENIED)) {
+					if ((hrt_elapsed_time(&last_auto_state_valid) > FAILSAFE_DEFAULT_TIMEOUT) && (auto_res == TRANSITION_DENIED)) {
 						/* AUTO mode denied, don't try RTL, switch to failsafe state LAND */
-						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+						auto_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);

-						if (res == TRANSITION_DENIED) {
+						if (auto_res == TRANSITION_DENIED) {
 							/* LAND not allowed, set TERMINATION state */
-							transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
-
-							if (res == TRANSITION_CHANGED) {
-								mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION");
-							}
-
-						} else if (res == TRANSITION_CHANGED) {
-							mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING");
+							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
 						}
 					} else {
 						status.set_nav_state = NAVIGATION_STATE_MISSION;
@@ -1269,47 +1320,37 @@ int commander_thread_main(int argc, char *argv[])

 				} else {
 					/* failsafe for manual modes */
-					transition_result_t res = TRANSITION_DENIED;
+					transition_result_t manual_res = TRANSITION_DENIED;

 					if (!status.condition_landed) {
 						/* vehicle is not landed, try to perform RTL */
-						res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
-
-						if (res == TRANSITION_CHANGED) {
-							mavlink_log_critical(mavlink_fd, "#a FAILSAFE: RETURN TO LAND");
-						}
+						manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL);
 					}

-					if (res == TRANSITION_DENIED) {
+					if (manual_res == TRANSITION_DENIED) {
 						/* RTL not allowed (no global position estimate) or not wanted, try LAND */
-						res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);
+						manual_res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND);

-						if (res == TRANSITION_CHANGED) {
-							mavlink_log_critical(mavlink_fd, "#a FAILSAFE: LANDING");
-						}
-
-						if (res == TRANSITION_DENIED) {
+						if (manual_res == TRANSITION_DENIED) {
 							/* LAND not allowed, set TERMINATION state */
-							res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
-						} else if (res == TRANSITION_CHANGED) {
-							mavlink_log_critical(mavlink_fd, "#a FAILSAFE: TERMINATION");
+							(void)failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION);
 						}
 					} else {
 						status.set_nav_state = NAVIGATION_STATE_RTL;
-					}	
+					}
 				}

 			} else {
 				if (status.failsafe_state != FAILSAFE_STATE_NORMAL) {
 					/* reset failsafe when disarmed */
-					transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
+					(void)failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL);
 				}
 			}
 		}

 		// TODO remove this hack
-		/* flight termination in manual mode if assisted switch is on easy position */
-		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) {
+		/* flight termination in manual mode if assist switch is on POSCTL position */
+		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctl_switch == SWITCH_POS_ON) {
 			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
 				tune_positive(armed.armed);
 			}
@@ -1323,8 +1364,9 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);

 			/* handle it */
-			if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub))
+			if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) {
 				status_changed = true;
+			}
 		}

 		/* check which state machines for changes, clear "changed" flag */
@@ -1341,7 +1383,7 @@ int commander_thread_main(int argc, char *argv[])

 			/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
 			if (armed.armed && !was_armed && hrt_absolute_time() > start_time + 2000000 && status.condition_global_position_valid &&
-				(global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {
+			    (global_position.eph < eph_epv_threshold) && (global_position.epv < eph_epv_threshold)) {

 				// TODO remove code duplication
 				home.lat = global_position.lat;
@@ -1353,7 +1395,7 @@ int commander_thread_main(int argc, char *argv[])
 				home.z = local_position.z;

 				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, "#audio: home: %.7f, %.7f, %.2f", home.lat, home.lon, (double)home.alt);

 				/* announce new home position */
 				if (home_pub > 0) {
@@ -1367,6 +1409,7 @@ int commander_thread_main(int argc, char *argv[])
 				status.condition_home_position_valid = true;
 			}
 		}
+
 		was_armed = armed.armed;

 		if (main_state_changed) {
@@ -1530,21 +1573,24 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a

 	} else if (actuator_armed->ready_to_arm) {
 		/* ready to arm, blink at 1Hz */
-		if (leds_counter % 20 == 0)
+		if (leds_counter % 20 == 0) {
 			led_toggle(LED_BLUE);
+		}

 	} else {
 		/* not ready to arm, blink at 10Hz */
-		if (leds_counter % 2 == 0)
+		if (leds_counter % 2 == 0) {
 			led_toggle(LED_BLUE);
+		}
 	}

 #endif

 	/* give system warnings on error LED, XXX maybe add memory usage warning too */
 	if (status->load > 0.95f) {
-		if (leds_counter % 2 == 0)
+		if (leds_counter % 2 == 0) {
 			led_toggle(LED_AMBER);
+		}

 	} else {
 		led_off(LED_AMBER);
@@ -1565,30 +1611,35 @@ 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;

-	case SWITCH_POS_MIDDLE:		// ASSISTED
-		if (sp_man->assisted_switch == SWITCH_POS_ON) {
-			res = main_state_transition(status, MAIN_STATE_EASY);
+	case SWITCH_POS_MIDDLE:		// ASSIST
+		if (sp_man->posctl_switch == SWITCH_POS_ON) {
+			res = main_state_transition(status, MAIN_STATE_POSCTL);

 			if (res != TRANSITION_DENIED) {
 				break;	// changed successfully or already in this state
 			}

-			// else fallback to SEATBELT
-			print_reject_mode(status, "EASY");
+			// else fallback to ALTCTL
+			print_reject_mode(status, "POSCTL");
 		}

-		res = main_state_transition(status, MAIN_STATE_SEATBELT);
+		res = main_state_transition(status, MAIN_STATE_ALTCTL);

 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this mode
 		}

-		if (sp_man->assisted_switch != SWITCH_POS_ON) {
-			print_reject_mode(status, "SEATBELT");
+		if (sp_man->posctl_switch != SWITCH_POS_ON) {
+			print_reject_mode(status, "ALTCTL");
 		}

 		// else fallback to MANUAL
@@ -1603,9 +1654,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin
 			break;	// changed successfully or already in this state
 		}

-		// else fallback to SEATBELT (EASY likely will not work too)
+		// else fallback to ALTCTL (POSCTL likely will not work too)
 		print_reject_mode(status, "AUTO");
-		res = main_state_transition(status, MAIN_STATE_SEATBELT);
+		res = main_state_transition(status, MAIN_STATE_ALTCTL);

 		if (res != TRANSITION_DENIED) {
 			break;	// changed successfully or already in this state
@@ -1651,7 +1702,7 @@ set_control_mode()
 			control_mode.flag_control_velocity_enabled = false;
 			break;

-		case MAIN_STATE_SEATBELT:
+		case MAIN_STATE_ALTCTL:
 			control_mode.flag_control_manual_enabled = true;
 			control_mode.flag_control_auto_enabled = false;
 			control_mode.flag_control_rates_enabled = true;
@@ -1662,7 +1713,7 @@ set_control_mode()
 			control_mode.flag_control_velocity_enabled = false;
 			break;

-		case MAIN_STATE_EASY:
+		case MAIN_STATE_POSCTL:
 			control_mode.flag_control_manual_enabled = true;
 			control_mode.flag_control_auto_enabled = false;
 			control_mode.flag_control_rates_enabled = true;
@@ -1675,6 +1726,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;
@@ -1758,7 +1821,7 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 {
 	switch (result) {
 	case VEHICLE_CMD_RESULT_ACCEPTED:
-			tune_positive(true);
+		tune_positive(true);
 		break;

 	case VEHICLE_CMD_RESULT_DENIED:
@@ -1772,7 +1835,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;

@@ -1808,8 +1872,9 @@ void *commander_low_prio_loop(void *arg)
 		int pret = poll(&fds[0], (sizeof(fds) / sizeof(fds[0])), 200);

 		/* timed out - periodic check for thread_should_exit, etc. */
-		if (pret == 0)
+		if (pret == 0) {
 			continue;
+		}

 		/* this is undesirable but not much we can do - might want to flag unhappy status */
 		if (pret < 0) {
@@ -1824,8 +1889,9 @@ void *commander_low_prio_loop(void *arg)
 		if (cmd.command == VEHICLE_CMD_DO_SET_MODE ||
 		    cmd.command == VEHICLE_CMD_COMPONENT_ARM_DISARM ||
 		    cmd.command == VEHICLE_CMD_NAV_TAKEOFF ||
-		    cmd.command == VEHICLE_CMD_DO_SET_SERVO)
+		    cmd.command == VEHICLE_CMD_DO_SET_SERVO) {
 			continue;
+		}

 		/* only handle low-priority commands here */
 		switch (cmd.command) {
@@ -1903,6 +1969,7 @@ void *commander_low_prio_loop(void *arg)
 					/* airspeed calibration */
 					answer_command(cmd, VEHICLE_CMD_RESULT_ACCEPTED);
 					calib_ret = do_airspeed_calibration(mavlink_fd);
+
 				} else if ((int)(cmd.param4) == 0) {
 					/* RC calibration ended - have we been in one worth confirming? */
 					if (status.rc_input_blocked) {
@@ -1917,10 +1984,12 @@ void *commander_low_prio_loop(void *arg)

 				}

-				if (calib_ret == OK)
+				if (calib_ret == OK) {
 					tune_positive(true);
-				else
+
+				} else {
 					tune_negative(true);
+				}

 				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);

@@ -1940,11 +2009,13 @@ void *commander_low_prio_loop(void *arg)
 						mavlink_log_critical(mavlink_fd, "#audio: parameters load ERROR");

 						/* convenience as many parts of NuttX use negative errno */
-						if (ret < 0)
+						if (ret < 0) {
 							ret = -ret;
+						}

-						if (ret < 1000)
+						if (ret < 1000) {
 							mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
+						}

 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
@@ -1960,11 +2031,13 @@ void *commander_low_prio_loop(void *arg)
 						mavlink_log_critical(mavlink_fd, "#audio: parameters save error");

 						/* convenience as many parts of NuttX use negative errno */
-						if (ret < 0)
+						if (ret < 0) {
 							ret = -ret;
+						}

-						if (ret < 1000)
+						if (ret < 1000) {
 							mavlink_log_critical(mavlink_fd, "#audio: %s", strerror(ret));
+						}

 						answer_command(cmd, VEHICLE_CMD_RESULT_FAILED);
 					}
@@ -1974,8 +2047,8 @@ void *commander_low_prio_loop(void *arg)
 			}

 		case VEHICLE_CMD_START_RX_PAIR:
-		/* handled in the IO driver */
-		break;
+			/* handled in the IO driver */
+			break;

 		default:
 			/* don't answer on unsupported commands, it will be done in main loop */
@@ -113,17 +113,22 @@ void buzzer_deinit()
 	close(buzzer);
 }

-void set_tune(int tune) {
+void set_tune(int tune)
+{
 	unsigned int new_tune_duration = tune_durations[tune];
+
 	/* don't interrupt currently playing non-repeating tune by repeating */
 	if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
 		/* allow interrupting current non-repeating tune by the same tune */
 		if (tune != tune_current || new_tune_duration != 0) {
 			ioctl(buzzer, TONE_SET_ALARM, tune);
 		}
+
 		tune_current = tune;
+
 		if (new_tune_duration != 0) {
 			tune_end = hrt_absolute_time() + new_tune_duration;
+
 		} else {
 			tune_end = 0;
 		}
@@ -138,6 +143,7 @@ void tune_positive(bool use_buzzer)
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_GREEN);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+
 	if (use_buzzer) {
 		set_tune(TONE_NOTIFY_POSITIVE_TUNE);
 	}
@@ -151,6 +157,7 @@ void tune_neutral(bool use_buzzer)
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_WHITE);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+
 	if (use_buzzer) {
 		set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
 	}
@@ -164,6 +171,7 @@ void tune_negative(bool use_buzzer)
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_RED);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
+
 	if (use_buzzer) {
 		set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
 	}
@@ -198,7 +206,7 @@ int led_init()
 		return ERROR;
 	}

-	/* the blue LED is only available on FMUv1 but not FMUv2 */
+	/* the blue LED is only available on FMUv1 & AeroCore but not FMUv2 */
 	(void)ioctl(leds, LED_ON, LED_BLUE);

 	/* we consider the amber led mandatory */
@@ -244,22 +252,25 @@ int led_off(int led)
 void rgbled_set_color(rgbled_color_t color)
 {

-	if (rgbleds != -1)
+	if (rgbleds != -1) {
 		ioctl(rgbleds, RGBLED_SET_COLOR, (unsigned long)color);
+	}
 }

 void rgbled_set_mode(rgbled_mode_t mode)
 {

-	if (rgbleds != -1)
+	if (rgbleds != -1) {
 		ioctl(rgbleds, RGBLED_SET_MODE, (unsigned long)mode);
+	}
 }

 void rgbled_set_pattern(rgbled_pattern_t *pattern)
 {

-	if (rgbleds != -1)
+	if (rgbleds != -1) {
 		ioctl(rgbleds, RGBLED_SET_PATTERN, (unsigned long)pattern);
+	}
 }

 float battery_remaining_estimate_voltage(float voltage, float discharged)
@@ -299,6 +310,7 @@ float battery_remaining_estimate_voltage(float voltage, float discharged)
 	if (bat_capacity > 0.0f) {
 		/* if battery capacity is known, use discharged current for estimate, but don't show more than voltage estimate */
 		ret = fminf(remaining_voltage, 1.0f - discharged / bat_capacity);
+
 	} else {
 		/* else use voltage */
 		ret = remaining_voltage;
@@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
 	ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state);

-	// MANUAL to SEATBELT.
+	// MANUAL to ALTCTRL.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_altitude_valid = true;
-	new_main_state = MAIN_STATE_SEATBELT;
-	ut_assert("tranisition: manual to seatbelt", 
+	new_main_state = MAIN_STATE_ALTCTL;
+	ut_assert("tranisition: manual to altctrl",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state);
+	ut_assert("new state: altctrl", MAIN_STATE_ALTCTL == current_state.main_state);

-	// MANUAL to SEATBELT, invalid local altitude.
+	// MANUAL to ALTCTRL, invalid local altitude.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_altitude_valid = false;
-	new_main_state = MAIN_STATE_SEATBELT;
+	new_main_state = MAIN_STATE_ALTCTL;
 	ut_assert("no transition: invalid local altitude",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
 	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);

-	// MANUAL to EASY.
+	// MANUAL to POSCTRL.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_position_valid = true;
-	new_main_state = MAIN_STATE_EASY;
-	ut_assert("transition: manual to easy",
+	new_main_state = MAIN_STATE_POSCTL;
+	ut_assert("transition: manual to posctrl",
 		  TRANSITION_CHANGED == main_state_transition(&current_state, new_main_state));
-	ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state);
+	ut_assert("current state: posctrl", MAIN_STATE_POSCTL == current_state.main_state);

-	// MANUAL to EASY, invalid local position.
+	// MANUAL to POSCTRL, invalid local position.
 	current_state.main_state = MAIN_STATE_MANUAL;
 	current_state.condition_local_position_valid = false;
-	new_main_state = MAIN_STATE_EASY;
+	new_main_state = MAIN_STATE_POSCTL;
 	ut_assert("no transition: invalid position",
 		  TRANSITION_DENIED == main_state_transition(&current_state, new_main_state));
 	ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state);
@@ -110,8 +110,9 @@ int do_gyro_calibration(int mavlink_fd)
 				gyro_scale.z_offset += gyro_report.z;
 				calibration_counter++;

-				if (calibration_counter % (calibration_count / 20) == 0)
+				if (calibration_counter % (calibration_count / 20) == 0) {
 					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+				}

 			} else {
 				poll_errcount++;
@@ -163,8 +164,9 @@ int do_gyro_calibration(int mavlink_fd)
 	/* apply new offsets */
 	fd = open(GYRO_DEVICE_PATH, 0);

-	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale))
+	if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale)) {
 		warn("WARNING: failed to apply new offsets for gyro");
+	}

 	close(fd);

@@ -178,9 +180,9 @@ int do_gyro_calibration(int mavlink_fd)

 	float mag_last = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);

-	if (mag_last > M_PI_F) mag_last -= 2 * M_PI_F;
+	if (mag_last > M_PI_F) { mag_last -= 2 * M_PI_F; }

-	if (mag_last < -M_PI_F) mag_last += 2 * M_PI_F;
+	if (mag_last < -M_PI_F) { mag_last += 2 * M_PI_F; }


 	uint64_t last_time = hrt_absolute_time();
@@ -220,15 +222,15 @@ int do_gyro_calibration(int mavlink_fd)
 			//float mag = -atan2f(magNav(1),magNav(0));
 			float mag = -atan2f(raw.magnetometer_ga[1], raw.magnetometer_ga[0]);

-			if (mag > M_PI_F) mag -= 2 * M_PI_F;
+			if (mag > M_PI_F) { mag -= 2 * M_PI_F; }

-			if (mag < -M_PI_F) mag += 2 * M_PI_F;
+			if (mag < -M_PI_F) { mag += 2 * M_PI_F; }

 			float diff = mag - mag_last;

-			if (diff > M_PI_F) diff -= 2 * M_PI_F;
+			if (diff > M_PI_F) { diff -= 2 * M_PI_F; }

-			if (diff < -M_PI_F) diff += 2 * M_PI_F;
+			if (diff < -M_PI_F) { diff += 2 * M_PI_F; }

 			baseline_integral += diff;
 			mag_last = mag;
@@ -72,7 +72,7 @@ int do_mag_calibration(int mavlink_fd)
 	uint64_t calibration_interval = 45 * 1000 * 1000;

 	/* maximum 500 values */
-	const unsigned int calibration_maxcount = 500;
+	const unsigned int calibration_maxcount = 240;
 	unsigned int calibration_counter;

 	struct mag_scale mscale_null = {
@@ -121,9 +121,24 @@ int do_mag_calibration(int mavlink_fd)

 		if (x == NULL || y == NULL || z == NULL) {
 			mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
+
+			/* clean up */
+			if (x != NULL) {
+				free(x);
+			}
+
+			if (y != NULL) {
+				free(y);
+			}
+
+			if (z != NULL) {
+				free(z);
+			}
+
 			res = ERROR;
 			return res;
 		}
+
 	} else {
 		/* exit */
 		return ERROR;
@@ -163,8 +178,9 @@ int do_mag_calibration(int mavlink_fd)

 				calibration_counter++;

-				if (calibration_counter % (calibration_maxcount / 20) == 0)
+				if (calibration_counter % (calibration_maxcount / 20) == 0) {
 					mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount);
+				}

 			} else {
 				poll_errcount++;
@@ -198,14 +214,17 @@ int do_mag_calibration(int mavlink_fd)
 		}
 	}

-	if (x != NULL)
+	if (x != NULL) {
 		free(x);
+	}

-	if (y != NULL)
+	if (y != NULL) {
 		free(y);
+	}

-	if (z != NULL)
+	if (z != NULL) {
 		free(z);
+	}

 	if (res == OK) {
 		/* apply calibration and set parameters */
@@ -234,23 +253,29 @@ int do_mag_calibration(int mavlink_fd)

 		if (res == OK) {
 			/* set parameters */
-			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset)))
+			if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) {
 				res = ERROR;
+			}

-			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset)))
+			if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) {
 				res = ERROR;
+			}

-			if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset)))
+			if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) {
 				res = ERROR;
+			}

-			if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale)))
+			if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) {
 				res = ERROR;
+			}

-			if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale)))
+			if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) {
 				res = ERROR;
+			}

-			if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale)))
+			if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) {
 				res = ERROR;
+			}

 			if (res != OK) {
 				mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
@@ -47,3 +47,7 @@ SRCS		 	= commander.cpp \
 			baro_calibration.cpp \
 			rc_calibration.cpp \
 			airspeed_calibration.cpp
+
+MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION	 = -Os
@@ -12,9 +12,10 @@

 enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_MANUAL = 1,
-	PX4_CUSTOM_MAIN_MODE_SEATBELT,
-	PX4_CUSTOM_MAIN_MODE_EASY,
+	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 {
@@ -69,11 +69,11 @@ int do_trim_calibration(int mavlink_fd)
 	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);

 	/* set parameters */
-	float p = sp.roll;
+	float p = sp.y;
 	param_set(param_find("TRIM_ROLL"), &p);
-	p = sp.pitch;
+	p = sp.x;
 	param_set(param_find("TRIM_PITCH"), &p);
-	p = sp.yaw;
+	p = sp.r;
 	param_set(param_find("TRIM_YAW"), &p);

 	/* store to permanent storage */
@@ -75,38 +75,38 @@ static bool failsafe_state_changed = true;
 // though the transition is marked as true additional checks must be made. See arming_state_transition
 // code for those checks.
 static const bool arming_transitions[ARMING_STATE_MAX][ARMING_STATE_MAX] = {
-    //                                  INIT,   STANDBY,    ARMED,  ARMED_ERROR,    STANDBY_ERROR,  REBOOT,     IN_AIR_RESTORE
-    { /* ARMING_STATE_INIT */           true,   true,       false,  false,          false,          false,      false },
-    { /* ARMING_STATE_STANDBY */        true,   true,       true,   true,           false,          false,      false },
-    { /* ARMING_STATE_ARMED */          false,  true,       true,   false,          false,          false,      true },
-    { /* ARMING_STATE_ARMED_ERROR */    false,  false,      true,   true,           false,          false,      false },
-    { /* ARMING_STATE_STANDBY_ERROR */  true,   true,       false,  true,           true,           false,      false },
-    { /* ARMING_STATE_REBOOT */         true,   true,       false,  false,          true,           true,       true },
-    { /* ARMING_STATE_IN_AIR_RESTORE */ false,  false,      false,  false,          false,          false,      false }, // NYI
+	//                                  INIT,   STANDBY,    ARMED,  ARMED_ERROR,    STANDBY_ERROR,  REBOOT,     IN_AIR_RESTORE
+	{ /* ARMING_STATE_INIT */           true,   true,       false,  false,          false,          false,      false },
+	{ /* ARMING_STATE_STANDBY */        true,   true,       true,   true,           false,          false,      false },
+	{ /* ARMING_STATE_ARMED */          false,  true,       true,   false,          false,          false,      true },
+	{ /* ARMING_STATE_ARMED_ERROR */    false,  false,      true,   true,           false,          false,      false },
+	{ /* ARMING_STATE_STANDBY_ERROR */  true,   true,       false,  true,           true,           false,      false },
+	{ /* ARMING_STATE_REBOOT */         true,   true,       false,  false,          true,           true,       true },
+	{ /* ARMING_STATE_IN_AIR_RESTORE */ false,  false,      false,  false,          false,          false,      false }, // NYI
 };

 // You can index into the array with an arming_state_t in order to get it's textual representation
-static const char* state_names[ARMING_STATE_MAX] = {
-    "ARMING_STATE_INIT",
-    "ARMING_STATE_STANDBY",
-    "ARMING_STATE_ARMED",
-    "ARMING_STATE_ARMED_ERROR",
-    "ARMING_STATE_STANDBY_ERROR",
-    "ARMING_STATE_REBOOT",
-    "ARMING_STATE_IN_AIR_RESTORE",
+static const char *state_names[ARMING_STATE_MAX] = {
+	"ARMING_STATE_INIT",
+	"ARMING_STATE_STANDBY",
+	"ARMING_STATE_ARMED",
+	"ARMING_STATE_ARMED_ERROR",
+	"ARMING_STATE_STANDBY_ERROR",
+	"ARMING_STATE_REBOOT",
+	"ARMING_STATE_IN_AIR_RESTORE",
 };

 transition_result_t
 arming_state_transition(struct vehicle_status_s *status,            /// current vehicle status
-                        const struct safety_s   *safety,            /// current safety settings
-                        arming_state_t          new_arming_state,   /// arming state requested
-                        struct actuator_armed_s *armed,             /// current armed status
-                        const int               mavlink_fd)         /// mavlink fd for error reporting, 0 for none
+			const struct safety_s   *safety,            /// current safety settings
+			arming_state_t          new_arming_state,   /// arming state requested
+			struct actuator_armed_s *armed,             /// current armed status
+			const int               mavlink_fd)         /// mavlink fd for error reporting, 0 for none
 {
-    // Double check that our static arrays are still valid
-    ASSERT(ARMING_STATE_INIT == 0);
-    ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
-    
+	// Double check that our static arrays are still valid
+	ASSERT(ARMING_STATE_INIT == 0);
+	ASSERT(ARMING_STATE_IN_AIR_RESTORE == ARMING_STATE_MAX - 1);
+
 	/*
 	 * Perform an atomic state update
 	 */
@@ -117,63 +117,70 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 	/* only check transition if the new state is actually different from the current one */
 	if (new_arming_state == status->arming_state) {
 		ret = TRANSITION_NOT_CHANGED;
+
 	} else {
 		/* enforce lockdown in HIL */
 		if (status->hil_state == HIL_STATE_ON) {
 			armed->lockdown = true;
+
 		} else {
 			armed->lockdown = false;
 		}
-        
-        // Check that we have a valid state transition
-        bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
-        if (valid_transition) {
-            // We have a good transition. Now perform any secondary validation.
-            if (new_arming_state == ARMING_STATE_ARMED) {
-                // Fail transition if we need safety switch press
-                //      Allow if coming from in air restore
-                //      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.");
-                    }
-                    valid_transition = false;
-                }
-            } else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
-                new_arming_state = ARMING_STATE_STANDBY_ERROR;
-            }
-        }
-        
-        // HIL can always go to standby
-        if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
-            valid_transition = true;
-        }
-        
-        /* Sensors need to be initialized for STANDBY state */
-        if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
-            valid_transition = false;
-        }

-        // Finish up the state transition
-        if (valid_transition) {
-            armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
-            armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
-            ret = TRANSITION_CHANGED;
-            status->arming_state = new_arming_state;
-            arming_state_changed = true;
-        }
-    }
-    
+		// Check that we have a valid state transition
+		bool valid_transition = arming_transitions[new_arming_state][status->arming_state];
+
+		if (valid_transition) {
+			// We have a good transition. Now perform any secondary validation.
+			if (new_arming_state == ARMING_STATE_ARMED) {
+				// Fail transition if we need safety switch press
+				//      Allow if coming from in air restore
+				//      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, "#audio: NOT ARMING: Press safety switch first.");
+					}
+
+					valid_transition = false;
+				}
+
+			} else if (new_arming_state == ARMING_STATE_STANDBY && status->arming_state == ARMING_STATE_ARMED_ERROR) {
+				new_arming_state = ARMING_STATE_STANDBY_ERROR;
+			}
+		}
+
+		// HIL can always go to standby
+		if (status->hil_state == HIL_STATE_ON && new_arming_state == ARMING_STATE_STANDBY) {
+			valid_transition = true;
+		}
+
+		/* Sensors need to be initialized for STANDBY state */
+		if (new_arming_state == ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
+			valid_transition = false;
+		}
+
+		// Finish up the state transition
+		if (valid_transition) {
+			armed->armed = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_ARMED_ERROR;
+			armed->ready_to_arm = new_arming_state == ARMING_STATE_ARMED || new_arming_state == ARMING_STATE_STANDBY;
+			ret = TRANSITION_CHANGED;
+			status->arming_state = new_arming_state;
+			arming_state_changed = true;
+		}
+	}
+
 	/* end of atomic state update */
 	irqrestore(flags);

-    if (ret == TRANSITION_DENIED) {
-        static const char* errMsg = "Invalid arming transition from %s to %s";
-        if (mavlink_fd) {
-            mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
-        }
-        warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
-    }
+	if (ret == TRANSITION_DENIED) {
+		static const char *errMsg = "Invalid arming transition from %s to %s";
+
+		if (mavlink_fd) {
+			mavlink_log_critical(mavlink_fd, errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+		}
+
+		warnx(errMsg, state_names[status->arming_state], state_names[new_arming_state]);
+	}

 	return ret;
 }
@@ -215,7 +222,11 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		ret = TRANSITION_CHANGED;
 		break;

-	case MAIN_STATE_SEATBELT:
+	case MAIN_STATE_ACRO:
+		ret = TRANSITION_CHANGED;
+		break;
+
+	case MAIN_STATE_ALTCTL:

 		/* need at minimum altitude estimate */
 		if (!status->is_rotary_wing ||
@@ -226,7 +237,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta

 		break;

-	case MAIN_STATE_EASY:
+	case MAIN_STATE_POSCTL:

 		/* need at minimum local position estimate */
 		if (status->condition_local_position_valid ||
@@ -301,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;
@@ -320,6 +331,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 				/* list directory */
 				DIR		*d;
 				d = opendir("/dev");
+
 				if (d) {

 					struct dirent	*direntry;
@@ -331,26 +343,32 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s
 						if (!strncmp("tty", direntry->d_name, 3)) {
 							continue;
 						}
+
 						/* skip mtd devices */
 						if (!strncmp("mtd", direntry->d_name, 3)) {
 							continue;
 						}
+
 						/* skip ram devices */
 						if (!strncmp("ram", direntry->d_name, 3)) {
 							continue;
 						}
+
 						/* skip MMC devices */
 						if (!strncmp("mmc", direntry->d_name, 3)) {
 							continue;
 						}
+
 						/* skip mavlink */
 						if (!strcmp("mavlink", direntry->d_name)) {
 							continue;
 						}
+
 						/* skip console */
 						if (!strcmp("console", direntry->d_name)) {
 							continue;
 						}
+
 						/* skip null */
 						if (!strcmp("null", direntry->d_name)) {
 							continue;
@@ -1,10 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: 	Jean Cyr
- *              Lorenz Meier
- *              Julian Oes
- *              Thomas Gubler
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -37,6 +33,11 @@
 /**
 * @file dataman.c
 * DATAMANAGER driver.
+ *
+ * @author Jean Cyr
+ * @author Lorenz Meier
+ * @author Julian Oes
+ * @author Thomas Gubler
 */

 #include <nuttx/config.h>
@@ -62,7 +63,7 @@ __EXPORT ssize_t dm_write(dm_item_t  item, unsigned char index, dm_persitence_t
 __EXPORT int dm_clear(dm_item_t item);
 __EXPORT int dm_restart(dm_reset_reason restart_type);

-/* Types of function calls supported by the worker task */
+/** Types of function calls supported by the worker task */
 typedef enum {
 	dm_write_func = 0,
 	dm_read_func,
@@ -71,11 +72,12 @@ typedef enum {
 	dm_number_of_funcs
 } dm_function_t;

-/* Work task work item */
+/** Work task work item */
 typedef struct {
 	sq_entry_t link;	/**< list linkage */
 	sem_t wait_sem;
-	dm_function_t func;
+	unsigned char first;
+	unsigned char func;
 	ssize_t result;
 	union {
 		struct {
@@ -100,6 +102,8 @@ typedef struct {
 	};
 } work_q_item_t;

+const size_t k_work_item_allocation_chunk_size = 8;
+
 /* Usage statistics */
 static unsigned g_func_counts[dm_number_of_funcs];

@@ -177,9 +181,23 @@ create_work_item(void)

 	unlock_queue(&g_free_q);

-	/* If we there weren't any free items then obtain memory for a new one */
-	if (item == NULL)
-		item = (work_q_item_t *)malloc(sizeof(work_q_item_t));
+	/* If we there weren't any free items then obtain memory for a new ones */
+	if (item == NULL) {
+		item = (work_q_item_t *)malloc(k_work_item_allocation_chunk_size * sizeof(work_q_item_t));
+		if (item) {
+			item->first = 1;
+			lock_queue(&g_free_q);
+			for (int i = 1; i < k_work_item_allocation_chunk_size; i++) {
+				(item + i)->first = 0;
+				sq_addfirst(&(item + i)->link, &(g_free_q.q));
+			}
+			/* Update the queue size and potentially the maximum queue size */
+			g_free_q.size += k_work_item_allocation_chunk_size - 1;
+			if (g_free_q.size > g_free_q.max_size)
+				g_free_q.max_size = g_free_q.size;
+			unlock_queue(&g_free_q);
+		}
+	}

 	/* If we got one then lock the item*/
 	if (item)
@@ -411,7 +429,7 @@ _clear(dm_item_t item)
 	return result;
 }

-/* Tell the data manager about the type of the last reset */
+/** Tell the data manager about the type of the last reset */
 static int
 _restart(dm_reset_reason reason)
 {
@@ -480,7 +498,7 @@ _restart(dm_reset_reason reason)
 	return result;
 }

-/* write to the data manager file */
+/** Write to the data manager file */
 __EXPORT ssize_t
 dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const void *buf, size_t count)
 {
@@ -505,7 +523,7 @@ dm_write(dm_item_t item, unsigned char index, dm_persitence_t persistence, const
 	return (ssize_t)enqueue_work_item_and_wait_for_result(work);
 }

-/* Retrieve from the data manager file */
+/** Retrieve from the data manager file */
 __EXPORT ssize_t
 dm_read(dm_item_t item, unsigned char index, void *buf, size_t count)
 {
@@ -717,8 +735,8 @@ task_main(int argc, char *argv[])
 	for (;;) {
 		if ((work = (work_q_item_t *)sq_remfirst(&(g_free_q.q))) == NULL)
 			break;
-
-		free(work);
+		if (work->first)
+			free(work);
 	}

 	destroy_q(&g_work_q);
@@ -736,7 +754,7 @@ start(void)
 	sem_init(&g_init_sema, 1, 0);

 	/* start the worker thread */
-	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2048, task_main, NULL)) <= 0) {
+	if ((task = task_spawn_cmd("dataman", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 2000, task_main, NULL)) <= 0) {
 		warn("task start failed");
 		return -1;
 	}
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -46,7 +46,7 @@
 extern "C" {
 #endif

-	/* Types of items that the data manager can store */
+	/** Types of items that the data manager can store */
 	typedef enum {
 		DM_KEY_SAFE_POINTS = 0,		/* Safe points coordinates, safe point 0 is home point */
 		DM_KEY_FENCE_POINTS,		/* Fence vertex coordinates */
@@ -56,7 +56,7 @@ extern "C" {
 		DM_KEY_NUM_KEYS			/* Total number of item types defined */
 	} dm_item_t;

-	/* The maximum number of instances for each item type */
+	/** The maximum number of instances for each item type */
 	enum {
 		DM_KEY_SAFE_POINTS_MAX = 8,
 		DM_KEY_FENCE_POINTS_MAX = GEOFENCE_MAX_VERTICES,
@@ -65,24 +65,24 @@ extern "C" {
 		DM_KEY_WAYPOINTS_ONBOARD_MAX = NUM_MISSIONS_SUPPORTED
 	};

-	/* Data persistence levels */
+	/** Data persistence levels */
 	typedef enum {
 		DM_PERSIST_POWER_ON_RESET = 0,	/* Data survives all resets */
 		DM_PERSIST_IN_FLIGHT_RESET,     /* Data survives in-flight resets only */
 		DM_PERSIST_VOLATILE             /* Data does not survive resets */
 	} dm_persitence_t;

-	/* The reason for the last reset */
+	/** The reason for the last reset */
 	typedef enum {
 		DM_INIT_REASON_POWER_ON = 0,	/* Data survives resets */
 		DM_INIT_REASON_IN_FLIGHT,		/* Data survives in-flight resets only */
 		DM_INIT_REASON_VOLATILE			/* Data does not survive reset */
 	} dm_reset_reason;

-	/* Maximum size in bytes of a single item instance */
+	/** Maximum size in bytes of a single item instance */
 	#define DM_MAX_DATA_SIZE 124

-	/* Retrieve from the data manager store */
+	/** Retrieve from the data manager store */
 	__EXPORT ssize_t
 	dm_read(
 		dm_item_t item,			/* The item type to retrieve */
@@ -91,7 +91,7 @@ extern "C" {
 		size_t buflen			/* Length in bytes of data to retrieve */
 	);

-	/* write to the data manager store */
+	/** write to the data manager store */
 	__EXPORT ssize_t
 	dm_write(
 		dm_item_t  item,		/* The item type to store */
@@ -101,13 +101,13 @@ extern "C" {
 		size_t buflen			/* Length in bytes of data to retrieve */
 	);

-	/* Erase all items of this type */
+	/** Erase all items of this type */
 	__EXPORT int
 	dm_clear(
 		dm_item_t item			/* The item type to clear */
 		);

-	/* Tell the data manager about the type of the last reset */
+	/** Tell the data manager about the type of the last reset */
 	__EXPORT int
 	dm_restart(
 		dm_reset_reason restart_type	/* The last reset type */
@@ -38,3 +38,5 @@
 MODULE_COMMAND	= dataman

 SRCS		= dataman.c
+
+MODULE_STACKSIZE = 1200
@@ -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>
@@ -97,6 +97,7 @@ __EXPORT uint32_t millis();

 static uint64_t last_run = 0;
 static uint64_t IMUmsec = 0;
+static const uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000;

 uint32_t millis()
 {
@@ -194,6 +195,7 @@ private:
 	bool						_baro_init;
 	bool						_gps_initialized;
 	uint64_t					_gps_start_time;
+	uint64_t					_filter_start_time;
 	bool						_gyro_valid;
 	bool						_accel_valid;
 	bool						_mag_valid;
@@ -246,7 +248,7 @@ private:

 	float						_velocity_xy_filtered;
 	float						_velocity_z_filtered;
-	float						_airspeed_filtered; 						
+	float						_airspeed_filtered;

 	/**
 	 * Update our local parameter cache.
@@ -272,7 +274,7 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 };

 namespace estimator
@@ -306,6 +308,8 @@ FixedwingEstimator::FixedwingEstimator() :
 	_vstatus_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
+	_mission_sub(-1),
+	_home_sub(-1),

 /* publications */
 	_att_pub(-1),
@@ -336,13 +340,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),
@@ -509,14 +513,14 @@ FixedwingEstimator::task_main_trampoline(int argc, char *argv[])
 	estimator::g_estimator->task_main();
 }

-float dt = 0.0f; // time lapsed since last covariance prediction
-
 void
 FixedwingEstimator::task_main()
 {
 	_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);

 	_ekf = new AttPosEKF();
+	float dt = 0.0f; // time lapsed since last covariance prediction
+	_filter_start_time = hrt_absolute_time();

 	if (!_ekf) {
 		errx(1, "failed allocating EKF filter - out of RAM!");
@@ -642,6 +646,7 @@ FixedwingEstimator::task_main()

 				_ekf->ZeroVariables();
 				_ekf->dtIMU = 0.01f;
+				_filter_start_time = last_sensor_timestamp;

 				/* now skip this loop and get data on the next one, which will also re-init the filter */
 				continue;
@@ -719,6 +724,8 @@ FixedwingEstimator::task_main()

 			if (last_accel != _sensor_combined.accelerometer_timestamp) {
 				accel_updated = true;
+			} else {
+				accel_updated = false;
 			}

 			last_accel = _sensor_combined.accelerometer_timestamp;
@@ -809,7 +816,6 @@ FixedwingEstimator::task_main()
 				perf_count(_perf_gps);

 				if (_gps.fix_type < 3) {
-					gps_updated = false;
 					newDataGps = false;

 				} else {
@@ -1025,7 +1031,7 @@ FixedwingEstimator::task_main()
 			 *    PART TWO: EXECUTE THE FILTER
 			 **/

-			if (_baro_init && _gyro_valid && _accel_valid && _mag_valid) {
+			if ((hrt_elapsed_time(&_filter_start_time) > FILTER_INIT_DELAY) && _baro_init && _gyro_valid && _accel_valid && _mag_valid) {

 				float initVelNED[3];

@@ -1042,9 +1048,9 @@ FixedwingEstimator::task_main()

 					// Set up height correctly
 					orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
-					_baro_gps_offset = gps_alt - _baro.altitude;
+					_baro_gps_offset = _baro_ref - _baro.altitude;
 					_ekf->baroHgt = _baro.altitude;
-					_ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref);
+					_ekf->hgtMea = 1.0f * (_ekf->baroHgt - (_baro_ref));

 					// Set up position variables correctly
 					_ekf->GPSstatus = _gps.fix_type;
@@ -1061,14 +1067,14 @@ FixedwingEstimator::task_main()
 					// Initialize projection
 					_local_pos.ref_lat = lat;
 					_local_pos.ref_lon = lon;
-					_local_pos.ref_alt = _baro_ref + _baro_gps_offset;
+					_local_pos.ref_alt = gps_alt;
 					_local_pos.ref_timestamp = _gps.timestamp_position;

 					map_projection_init(&_pos_ref, lat, lon);
 					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", _ekf->baroHgt, _ekf->hgtMea);
+					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, (double)_gps.epv, (double)math::degrees(declination));

 					_gps_initialized = true;
@@ -1084,6 +1090,10 @@ FixedwingEstimator::task_main()

 					_ekf->posNE[0] = _ekf->posNED[0];
 					_ekf->posNE[1] = _ekf->posNED[1];
+
+					_local_pos.ref_alt = _baro_ref;
+					_baro_gps_offset = 0.0f;
+
 					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
 				}
 			}
@@ -1263,7 +1273,8 @@ FixedwingEstimator::task_main()
 				_local_pos.timestamp = last_sensor_timestamp;
 				_local_pos.x = _ekf->states[7];
 				_local_pos.y = _ekf->states[8];
-				_local_pos.z = _ekf->states[9] + _baro_gps_offset;
+				// XXX need to announce change of Z reference somehow elegantly
+				_local_pos.z = _ekf->states[9] - _baro_gps_offset;

 				_local_pos.vx = _ekf->states[4];
 				_local_pos.vy = _ekf->states[5];
@@ -1324,8 +1335,8 @@ FixedwingEstimator::task_main()
 					_global_pos.vel_e = 0.0f;
 				}

-				/* local pos alt is negative, change sign and add alt offset */
-				_global_pos.alt = _local_pos.ref_alt + (-_local_pos.z);
+				/* local pos alt is negative, change sign and add alt offsets */
+				_global_pos.alt = _local_pos.ref_alt + _baro_gps_offset + (-_local_pos.z);

 				if (_local_pos.v_z_valid) {
 					_global_pos.vel_d = _local_pos.vz;
@@ -1400,7 +1411,8 @@ FixedwingEstimator::print_status()
 	// 15-17: Earth Magnetic Field Vector - gauss (North, East, Down)
 	// 18-20: Body Magnetic Field Vector - gauss (X,Y,Z)

-	printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)dt, (int)IMUmsec);
+	printf("dtIMU: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (int)IMUmsec);
+	printf("ref alt: %8.6f\n", (double)_local_pos.ref_alt);
 	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
 	printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z);
 	printf("states (quat)        [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]);
@@ -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
@@ -1,5 +1,6 @@
 #include "estimator.h"
 #include <string.h>
+#include <stdarg.h>

 // Define EKF_DEBUG here to enable the debug print calls
 // if the macro is not set, these will be completely
@@ -1268,7 +1269,7 @@ void AttPosEKF::FuseMagnetometer()
    for (uint8_t i = 0; i < n_states; i++) {
        H_MAG[i] = 0.0f;
    }
-    uint8_t indexLimit;
+    unsigned indexLimit;

 // Perform sequential fusion of Magnetometer measurements.
 // This assumes that the errors in the different components are
@@ -1281,11 +1282,11 @@ void AttPosEKF::FuseMagnetometer()
        // Limit range of states modified when on ground
        if(!onGround)
        {
-            indexLimit = 22;
+            indexLimit = n_states;
        }
        else
        {
-            indexLimit = 13;
+            indexLimit = 13 + 1;
        }

        // Sequential fusion of XYZ components to spread processing load across
@@ -1346,7 +1347,16 @@ void AttPosEKF::FuseMagnetometer()
            H_MAG[19] = 1.0f;

            // Calculate Kalman gain
-            SK_MX[0] = 1/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            float temp = (P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            if (temp >= R_MAG) {
+                SK_MX[0] = 1.0f / temp;
+            } else {
+                // the calculation is badly conditioned, so we cannot perform fusion on this step
+                // we increase the state variances and try again next time
+                P[19][19] += 0.1f*R_MAG;
+                obsIndex = 1;
+                return;
+            }
            SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6];
            SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2;
            SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
@@ -1398,7 +1408,16 @@ void AttPosEKF::FuseMagnetometer()
            H_MAG[20] = 1;

            // Calculate Kalman gain
-            SK_MY[0] = 1/(P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            float temp = (P[20][20] + R_MAG + P[0][20]*SH_MAG[2] + P[1][20]*SH_MAG[1] + P[2][20]*SH_MAG[0] - P[17][20]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - (2*q0*q3 - 2*q1*q2)*(P[20][16] + P[0][16]*SH_MAG[2] + P[1][16]*SH_MAG[1] + P[2][16]*SH_MAG[0] - P[17][16]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][16]*(2*q0*q3 - 2*q1*q2) + P[18][16]*(2*q0*q1 + 2*q2*q3) - P[3][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (2*q0*q1 + 2*q2*q3)*(P[20][18] + P[0][18]*SH_MAG[2] + P[1][18]*SH_MAG[1] + P[2][18]*SH_MAG[0] - P[17][18]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][18]*(2*q0*q3 - 2*q1*q2) + P[18][18]*(2*q0*q1 + 2*q2*q3) - P[3][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[20][3] + P[0][3]*SH_MAG[2] + P[1][3]*SH_MAG[1] + P[2][3]*SH_MAG[0] - P[17][3]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][3]*(2*q0*q3 - 2*q1*q2) + P[18][3]*(2*q0*q1 + 2*q2*q3) - P[3][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[16][20]*(2*q0*q3 - 2*q1*q2) + P[18][20]*(2*q0*q1 + 2*q2*q3) + SH_MAG[2]*(P[20][0] + P[0][0]*SH_MAG[2] + P[1][0]*SH_MAG[1] + P[2][0]*SH_MAG[0] - P[17][0]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][0]*(2*q0*q3 - 2*q1*q2) + P[18][0]*(2*q0*q1 + 2*q2*q3) - P[3][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[20][1] + P[0][1]*SH_MAG[2] + P[1][1]*SH_MAG[1] + P[2][1]*SH_MAG[0] - P[17][1]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][1]*(2*q0*q3 - 2*q1*q2) + P[18][1]*(2*q0*q1 + 2*q2*q3) - P[3][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[20][2] + P[0][2]*SH_MAG[2] + P[1][2]*SH_MAG[1] + P[2][2]*SH_MAG[0] - P[17][2]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][2]*(2*q0*q3 - 2*q1*q2) + P[18][2]*(2*q0*q1 + 2*q2*q3) - P[3][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6])*(P[20][17] + P[0][17]*SH_MAG[2] + P[1][17]*SH_MAG[1] + P[2][17]*SH_MAG[0] - P[17][17]*(SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6]) - P[16][17]*(2*q0*q3 - 2*q1*q2) + P[18][17]*(2*q0*q1 + 2*q2*q3) - P[3][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - P[3][20]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            if (temp >= R_MAG) {
+                SK_MY[0] = 1.0f / temp;
+            } else {
+                // the calculation is badly conditioned, so we cannot perform fusion on this step
+                // we increase the state variances and try again next time
+                P[20][20] += 0.1f*R_MAG;
+                obsIndex = 2;
+                return;
+            }
            SK_MY[1] = SH_MAG[3] - SH_MAG[4] + SH_MAG[5] - SH_MAG[6];
            SK_MY[2] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
            SK_MY[3] = 2*q0*q3 - 2*q1*q2;
@@ -1444,7 +1463,16 @@ void AttPosEKF::FuseMagnetometer()
            H_MAG[21] = 1;

            // Calculate Kalman gain
-            SK_MZ[0] = 1/(P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            float temp = (P[21][21] + R_MAG + P[0][21]*SH_MAG[1] + P[3][21]*SH_MAG[0] + P[18][21]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) - (2*magD*q1 + 2*magE*q0 - 2*magN*q3)*(P[21][1] + P[0][1]*SH_MAG[1] + P[3][1]*SH_MAG[0] + P[18][1]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][1]*(2*q0*q2 + 2*q1*q3) - P[17][1]*(2*q0*q1 - 2*q2*q3) - P[1][1]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[21][2] + P[0][2]*SH_MAG[1] + P[3][2]*SH_MAG[0] + P[18][2]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][2]*(2*q0*q2 + 2*q1*q3) - P[17][2]*(2*q0*q1 - 2*q2*q3) - P[1][2]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[1]*(P[21][0] + P[0][0]*SH_MAG[1] + P[3][0]*SH_MAG[0] + P[18][0]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][0]*(2*q0*q2 + 2*q1*q3) - P[17][0]*(2*q0*q1 - 2*q2*q3) - P[1][0]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[21][3] + P[0][3]*SH_MAG[1] + P[3][3]*SH_MAG[0] + P[18][3]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][3]*(2*q0*q2 + 2*q1*q3) - P[17][3]*(2*q0*q1 - 2*q2*q3) - P[1][3]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6])*(P[21][18] + P[0][18]*SH_MAG[1] + P[3][18]*SH_MAG[0] + P[18][18]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][18]*(2*q0*q2 + 2*q1*q3) - P[17][18]*(2*q0*q1 - 2*q2*q3) - P[1][18]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][21]*(2*q0*q2 + 2*q1*q3) - P[17][21]*(2*q0*q1 - 2*q2*q3) - P[1][21]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + (2*q0*q2 + 2*q1*q3)*(P[21][16] + P[0][16]*SH_MAG[1] + P[3][16]*SH_MAG[0] + P[18][16]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][16]*(2*q0*q2 + 2*q1*q3) - P[17][16]*(2*q0*q1 - 2*q2*q3) - P[1][16]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q1 - 2*q2*q3)*(P[21][17] + P[0][17]*SH_MAG[1] + P[3][17]*SH_MAG[0] + P[18][17]*(SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6]) + P[16][17]*(2*q0*q2 + 2*q1*q3) - P[17][17]*(2*q0*q1 - 2*q2*q3) - P[1][17]*(2*magD*q1 + 2*magE*q0 - 2*magN*q3) + P[2][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[2][21]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2));
+            if (temp >= R_MAG) {
+                SK_MZ[0] = 1.0f / temp;
+            } else {
+                // the calculation is badly conditioned, so we cannot perform fusion on this step
+                // we increase the state variances and try again next time
+                P[21][21] += 0.1f*R_MAG;
+                obsIndex = 3;
+                return;
+            }
            SK_MZ[1] = SH_MAG[3] - SH_MAG[4] - SH_MAG[5] + SH_MAG[6];
            SK_MZ[2] = 2*magD*q1 + 2*magE*q0 - 2*magN*q3;
            SK_MZ[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2;
@@ -1483,7 +1511,7 @@ void AttPosEKF::FuseMagnetometer()
        if ((innovMag[obsIndex]*innovMag[obsIndex]/varInnovMag[obsIndex]) < 25.0)
        {
            // correct the state vector
-            for (uint8_t j= 0; j<=indexLimit; j++)
+            for (uint8_t j= 0; j < indexLimit; j++)
            {
                states[j] = states[j] - Kfusion[j] * innovMag[obsIndex];
            }
@@ -1500,7 +1528,7 @@ void AttPosEKF::FuseMagnetometer()
            // correct the covariance P = (I - K*H)*P
            // take advantage of the empty columns in KH to reduce the
            // number of operations
-            for (uint8_t i = 0; i<=indexLimit; i++)
+            for (uint8_t i = 0; i < indexLimit; i++)
            {
                for (uint8_t j = 0; j <= 3; j++)
                {
@@ -1522,9 +1550,9 @@ void AttPosEKF::FuseMagnetometer()
                    }
                }
            }
-            for (uint8_t i = 0; i<=indexLimit; i++)
+            for (uint8_t i = 0; i < indexLimit; i++)
            {
-                for (uint8_t j = 0; j<=indexLimit; j++)
+                for (uint8_t j = 0; j < indexLimit; j++)
                {
                    KHP[i][j] = 0.0f;
                    for (uint8_t k = 0; k <= 3; k++)
@@ -1541,9 +1569,9 @@ void AttPosEKF::FuseMagnetometer()
                }
            }
        }
-        for (uint8_t i = 0; i <= indexLimit; i++)
+        for (uint8_t i = 0; i < indexLimit; i++)
        {
-            for (uint8_t j = 0; j <= indexLimit; j++)
+            for (uint8_t j = 0; j < indexLimit; j++)
            {
                P[i][j] = P[i][j] - KHP[i][j];
            }
@@ -1564,6 +1592,7 @@ void AttPosEKF::FuseAirspeed()
    float vwe;
    float R_TAS = sq(airspeedMeasurementSigma);
    float SH_TAS[3];
+    float SK_TAS;
    float VtasPred;

    // Copy required states to local variable names
@@ -1593,7 +1622,16 @@ void AttPosEKF::FuseAirspeed()
        H_TAS[15] = -SH_TAS[1];

        // Calculate Kalman gains
-        float SK_TAS = 1/(R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
+        float temp = (R_TAS + SH_TAS[2]*(P[4][4]*SH_TAS[2] + P[5][4]*SH_TAS[1] - P[14][4]*SH_TAS[2] - P[15][4]*SH_TAS[1] + P[6][4]*vd*SH_TAS[0]) + SH_TAS[1]*(P[4][5]*SH_TAS[2] + P[5][5]*SH_TAS[1] - P[14][5]*SH_TAS[2] - P[15][5]*SH_TAS[1] + P[6][5]*vd*SH_TAS[0]) - SH_TAS[2]*(P[4][14]*SH_TAS[2] + P[5][14]*SH_TAS[1] - P[14][14]*SH_TAS[2] - P[15][14]*SH_TAS[1] + P[6][14]*vd*SH_TAS[0]) - SH_TAS[1]*(P[4][15]*SH_TAS[2] + P[5][15]*SH_TAS[1] - P[14][15]*SH_TAS[2] - P[15][15]*SH_TAS[1] + P[6][15]*vd*SH_TAS[0]) + vd*SH_TAS[0]*(P[4][6]*SH_TAS[2] + P[5][6]*SH_TAS[1] - P[14][6]*SH_TAS[2] - P[15][6]*SH_TAS[1] + P[6][6]*vd*SH_TAS[0]));
+        if (temp >= R_TAS) {
+            SK_TAS = 1.0f / temp;
+        } else {
+            // the calculation is badly conditioned, so we cannot perform fusion on this step
+            // we increase the wind state variances and try again next time
+            P[14][14] += 0.05f*R_TAS;
+            P[15][15] += 0.05f*R_TAS;
+            return;
+        }
        Kfusion[0] = SK_TAS*(P[0][4]*SH_TAS[2] - P[0][14]*SH_TAS[2] + P[0][5]*SH_TAS[1] - P[0][15]*SH_TAS[1] + P[0][6]*vd*SH_TAS[0]);
        Kfusion[1] = SK_TAS*(P[1][4]*SH_TAS[2] - P[1][14]*SH_TAS[2] + P[1][5]*SH_TAS[1] - P[1][15]*SH_TAS[1] + P[1][6]*vd*SH_TAS[0]);
        Kfusion[2] = SK_TAS*(P[2][4]*SH_TAS[2] - P[2][14]*SH_TAS[2] + P[2][5]*SH_TAS[1] - P[2][15]*SH_TAS[1] + P[2][6]*vd*SH_TAS[0]);
@@ -2001,7 +2039,22 @@ void AttPosEKF::CovarianceInit()

 float AttPosEKF::ConstrainFloat(float val, float min, float max)
 {
-    return (val > max) ? max : ((val < min) ? min : val);
+    float ret;
+    if (val > max) {
+        ret = max;
+        ekf_debug("> max: %8.4f, val: %8.4f", max, val);
+    } else if (val < min) {
+        ret = min;
+        ekf_debug("< min: %8.4f, val: %8.4f", min, val);
+    } else {
+        ret = val;
+    }
+
+    if (!isfinite(val)) {
+        ekf_debug("constrain: non-finite!");
+    }
+
+    return ret;
 }

 void AttPosEKF::ConstrainVariances()
@@ -2466,12 +2519,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination)

 void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination)
 {
-    //store initial lat,long and height
+    // store initial lat,long and height
    latRef = referenceLat;
    lonRef = referenceLon;
    hgtRef = referenceHgt;
    refSet = true;

+    // we are at reference altitude, so measurement must be zero
+    hgtMea = 0.0f;
+
+    // the baro offset must be this difference now
+    baroHgtOffset = baroHgt - referenceHgt;
+
    memset(&last_ekf_error, 0, sizeof(last_ekf_error));

    InitializeDynamic(initvelNED, declination);
@@ -198,6 +198,7 @@ public:
    float velNED[3]; // North, East, Down velocity obs (m/s)
    float posNE[2]; // North, East position obs (m)
    float hgtMea; //  measured height (m)
+    float baroHgtOffset;        ///< the baro (weather) offset from normalized altitude
    float rngMea; // Ground distance
    float posNED[3]; // North, East Down position (m)

@@ -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
@@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update()
 		_actuators.control[CH_RDR] = _manual.yaw;
 		_actuators.control[CH_THR] = _manual.throttle;

-	} else if (_status.main_state == MAIN_STATE_SEATBELT ||
-		_status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) {
+	} else if (_status.main_state == MAIN_STATE_ALTCTL ||
+		_status.main_state == MAIN_STATE_POSCTL /* TODO, implement pos control */) {

 		// calculate velocity, XXX should be airspeed, but using ground speed for now
 		// for the purpose of control we will limit the velocity feedback between
@@ -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 */

@@ -273,7 +275,7 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 };

 namespace att_control
@@ -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;
 				}
@@ -706,20 +718,21 @@ FixedwingAttitudeControl::task_main()
 				} else {
 					/*
 					 * Scale down roll and pitch as the setpoints are radians
-					 * and a typical remote can only do 45 degrees, the mapping is
-					 * -1..+1 to -45..+45 degrees or -0.75..+0.75 radians.
+					 * and a typical remote can only do around 45 degrees, the mapping is
+					 * -1..+1 to -man_roll_max rad..+man_roll_max rad (equivalent for pitch)
 					 *
 					 * With this mapping the stick angle is a 1:1 representation of
-					 * the commanded attitude. If more than 45 degrees are desired,
-					 * a scaling parameter can be applied to the remote.
+					 * the commanded attitude.
 					 *
 					 * The trim gets subtracted here from the manual setpoint to get
 					 * the intended attitude setpoint. Later, after the rate control step the
 					 * trim is added again to the control signal.
 					 */
-					roll_sp = (_manual.roll * _parameters.man_roll_max - _parameters.trim_roll) + _parameters.rollsp_offset_rad;
-					pitch_sp = (-_manual.pitch * _parameters.man_pitch_max - _parameters.trim_pitch) + _parameters.pitchsp_offset_rad;
-					throttle_sp = _manual.throttle;
+					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+						+ _parameters.rollsp_offset_rad;
+					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
+						+ _parameters.pitchsp_offset_rad;
+					throttle_sp = _manual.z;
 					_actuators.control[4] = _manual.flaps;

 					/*
@@ -754,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 */
@@ -772,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,
@@ -781,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,
@@ -791,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);
+					}
 				}

 				/*
@@ -825,10 +868,10 @@ FixedwingAttitudeControl::task_main()

 			} else {
 				/* manual/direct control */
-				_actuators.control[0] = _manual.roll;
-				_actuators.control[1] = -_manual.pitch;
-				_actuators.control[2] = _manual.yaw;
-				_actuators.control[3] = _manual.throttle;
+				_actuators.control[0] = _manual.y;
+				_actuators.control[1] = -_manual.x;
+				_actuators.control[2] = _manual.r;
+				_actuators.control[3] = _manual.z;
 				_actuators.control[4] = _manual.flaps;
 			}

@@ -864,6 +907,7 @@ FixedwingAttitudeControl::task_main()

 		}

+		loop_counter++;
 		perf_end(_loop_perf);
 	}

@@ -89,6 +89,7 @@
 #include <launchdetection/LaunchDetector.h>
 #include <ecl/l1/ecl_l1_pos_controller.h>
 #include <external_lgpl/tecs/tecs.h>
+#include <drivers/drv_range_finder.h>
 #include "landingslope.h"


@@ -134,6 +135,7 @@ private:
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
 	int		_sensor_combined_sub;		/**< for body frame accelerations */
+	int		_range_finder_sub;		/**< range finder subscription */

 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
@@ -147,11 +149,12 @@ private:
 	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
 	struct position_setpoint_triplet_s		_pos_sp_triplet;		/**< triplet of mission items */
 	struct sensor_combined_s			_sensor_combined;		/**< for body frame accelerations */
+	struct range_finder_report 			_range_finder;			/**< range finder report */

 	perf_counter_t	_loop_perf;			/**< loop performance counter */

 	/** manual control states */
-	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
+	float		_altctrl_hold_heading;		/**< heading the system should hold in altctrl mode */
 	double		_loiter_hold_lat;
 	double		_loiter_hold_lon;
 	float		_loiter_hold_alt;
@@ -179,7 +182,7 @@ private:
 	/* Landingslope object */
 	Landingslope landingslope;

-	float flare_curve_alt_last;
+	float flare_curve_alt_rel_last;
 	/* heading hold */
 	float target_bearing;

@@ -237,6 +240,7 @@ private:
 		float land_flare_alt_relative;
 		float land_thrust_lim_alt_relative;
 		float land_heading_hold_horizontal_distance;
+		float range_finder_rel_alt;

 	}		_parameters;			/**< local copies of interesting parameters */

@@ -281,6 +285,7 @@ private:
 		param_t land_flare_alt_relative;
 		param_t land_thrust_lim_alt_relative;
 		param_t land_heading_hold_horizontal_distance;
+		param_t range_finder_rel_alt;

 	}		_parameter_handles;		/**< handles for interesting parameters */

@@ -306,6 +311,12 @@ private:
 	 */
 	bool		vehicle_airspeed_poll();

+	/**
+	 * Check for range finder updates.
+	 */
+	bool		range_finder_poll();
+
+
 	/**
 	 * Check for position updates.
 	 */
@@ -326,6 +337,11 @@ private:
 	 */
 	void navigation_capabilities_publish();

+	/**
+	 * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder
+	 */
+	float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt);
+
 	/**
 	 * Control position.
 	 */
@@ -343,7 +359,7 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();

 	/*
 	 * Reset takeoff state
@@ -382,6 +398,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_control_mode_sub(-1),
 	_params_sub(-1),
 	_manual_control_sub(-1),
+	_range_finder_sub(-1),

 /* publications */
 	_attitude_sp_pub(-1),
@@ -398,9 +415,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_motor_lim(false),
 	land_onslope(false),
 	launch_detected(false),
-	last_manual(false),
 	usePreTakeoffThrust(false),
-	flare_curve_alt_last(0.0f),
+	last_manual(false),
+	flare_curve_alt_rel_last(0.0f),
 	launchDetector(),
 	_airspeed_error(0.0f),
 	_airspeed_valid(false),
@@ -414,7 +431,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_control_mode(),
 	_global_pos(),
 	_pos_sp_triplet(),
-	_sensor_combined()
+	_sensor_combined(),
+	_range_finder()
 {
 	_nav_capabilities.turn_distance = 0.0f;

@@ -439,6 +457,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
 	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
 	_parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST");
+	_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");

 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
 	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
@@ -528,6 +547,8 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
 	param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance));

+	param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt));
+
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
@@ -623,6 +644,20 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 	return airspeed_updated;
 }

+bool
+FixedwingPositionControl::range_finder_poll()
+{
+	/* check if there is a range finder measurement */
+	bool range_finder_updated;
+	orb_check(_range_finder_sub, &range_finder_updated);
+
+	if (range_finder_updated) {
+		orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder);
+	}
+
+	return range_finder_updated;
+}
+
 void
 FixedwingPositionControl::vehicle_attitude_poll()
 {
@@ -750,6 +785,23 @@ void FixedwingPositionControl::navigation_capabilities_publish()
 	}
 }

+float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt)
+{
+	float rel_alt_estimated = current_alt - land_setpoint_alt;
+
+	/* only use range finder if:
+	 * parameter (range_finder_use_relative_alt) > 0
+	 * the measurement is valid
+	 * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt
+	 */
+	if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) {
+		return rel_alt_estimated;
+	}
+
+	return range_finder.distance;
+
+}
+
 bool
 FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed,
 		const struct position_setpoint_triplet_s &pos_sp_triplet)
@@ -892,12 +944,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi

 			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
 			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
-			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
+			float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance);

 			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
-			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
+			float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);

-			if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out
+			float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt);
+
+			if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) {  //checking for land_noreturn to avoid unwanted climb out

 				/* land with minimal speed */

@@ -907,7 +961,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				/* kill the throttle if param requests it */
 				throttle_max = _parameters.throttle_max;

-				 if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) {
+				 if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) {
 					throttle_max = math::min(throttle_max, _parameters.throttle_land_max);
 					if (!land_motor_lim) {
 						land_motor_lim  = true;
@@ -916,16 +970,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi

 				 }

-				float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
+				float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp);

 				/* avoid climbout */
-				if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground)
+				if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground)
 				{
-					flare_curve_alt = pos_sp_triplet.current.alt;
+					flare_curve_alt_rel = 0.0f; // stay on ground
 					land_stayonground = true;
 				}

-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land),
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land),
 												    _airspeed.indicated_airspeed_m_s, eas2tas,
 												    false, flare_pitch_angle_rad,
 												    0.0f, throttle_max, throttle_land,
@@ -937,7 +991,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				}
 				//warnx("Landing:  flare, _global_pos.alt  %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);

-				flare_curve_alt_last = flare_curve_alt;
+				flare_curve_alt_rel_last = flare_curve_alt_rel;
 			} else {

 				 /* intersect glide slope:
@@ -945,20 +999,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				  * if current position is higher or within 10m of slope follow the glide slope
 				  * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
 				  * */
-				float altitude_desired = _global_pos.alt;
-				if (_global_pos.alt > landing_slope_alt_desired - 10.0f) {
+				float altitude_desired_rel = relative_alt;
+				if (relative_alt > landing_slope_alt_rel_desired - 10.0f) {
 					/* stay on slope */
-					altitude_desired = landing_slope_alt_desired;
+					altitude_desired_rel = landing_slope_alt_rel_desired;
 					if (!land_onslope) {
 						mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
 						land_onslope = true;
 					}
 				} else {
 					/* continue horizontally */
-					altitude_desired =  math::max(_global_pos.alt, L_altitude);
+					altitude_desired_rel =  math::max(relative_alt, L_altitude_rel);
 				}

-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach),
+				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach),
 							    _airspeed.indicated_airspeed_m_s, eas2tas,
 							    false, math::radians(_parameters.pitch_limit_min),
 							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -1047,16 +1101,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_reset_integral = true;
 		}

-	} else if (0/* easy mode enabled */) {
+	} else if (0/* posctrl mode enabled */) {

-		/** EASY FLIGHT **/
+		/** POSCTRL FLIGHT **/

-		if (0/* switched from another mode to easy */) {
-			_seatbelt_hold_heading = _att.yaw;
+		if (0/* switched from another mode to posctrl */) {
+			_altctrl_hold_heading = _att.yaw;
 		}

-		if (0/* easy on and manual control yaw non-zero */) {
-			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
+		if (0/* posctrl on and manual control yaw non-zero */) {
+			_altctrl_hold_heading = _att.yaw + _manual.r;
 		}

 		//XXX not used
@@ -1069,44 +1123,44 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 //			climb_out = true;
 //		}

-		/* if in seatbelt mode, set airspeed based on manual control */
+		/* if in altctrl mode, set airspeed based on manual control */

 		// XXX check if ground speed undershoot should be applied here
-		float seatbelt_airspeed = _parameters.airspeed_min +
+		float altctrl_airspeed = _parameters.airspeed_min +
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
-					  _manual.throttle;
+					  _manual.z;

-		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
+		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
 		_att_sp.roll_body = _l1_control.nav_roll();
 		_att_sp.yaw_body = _l1_control.nav_bearing();
-		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
-					    seatbelt_airspeed,
+		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f,
+					    altctrl_airspeed,
 					    _airspeed.indicated_airspeed_m_s, eas2tas,
 					    false, _parameters.pitch_limit_min,
 					    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
 					    _parameters.pitch_limit_min, _parameters.pitch_limit_max);

-	} else if (0/* seatbelt mode enabled */) {
+	} else if (0/* altctrl mode enabled */) {

-		/** SEATBELT FLIGHT **/
+		/** ALTCTRL FLIGHT **/

-		if (0/* switched from another mode to seatbelt */) {
-			_seatbelt_hold_heading = _att.yaw;
+		if (0/* switched from another mode to altctrl */) {
+			_altctrl_hold_heading = _att.yaw;
 		}

-		if (0/* seatbelt on and manual control yaw non-zero */) {
-			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
+		if (0/* altctrl on and manual control yaw non-zero */) {
+			_altctrl_hold_heading = _att.yaw + _manual.r;
 		}

-		/* if in seatbelt mode, set airspeed based on manual control */
+		/* if in altctrl mode, set airspeed based on manual control */

 		// XXX check if ground speed undershoot should be applied here
-		float seatbelt_airspeed = _parameters.airspeed_min +
+		float altctrl_airspeed = _parameters.airspeed_min +
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
-					  _manual.throttle;
+					  _manual.z;

 		/* user switched off throttle */
-		if (_manual.throttle < 0.1f) {
+		if (_manual.z < 0.1f) {
 			throttle_max = 0.0f;
 			/* switch to pure pitch based altitude control, give up speed */
 			_tecs.set_speed_weight(0.0f);
@@ -1116,15 +1170,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		bool climb_out = false;

 		/* user wants to climb out */
-		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+		if (_manual.x > 0.3f && _manual.z > 0.8f) {
 			climb_out = true;
 		}

-		_l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed);
-		_att_sp.roll_body =	_manual.roll;
-		_att_sp.yaw_body =	_manual.yaw;
-		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f,
-					    seatbelt_airspeed,
+		_l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed);
+		_att_sp.roll_body =	_manual.y;
+		_att_sp.yaw_body =	_manual.r;
+		_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.x * 2.0f,
+					    altctrl_airspeed,
 					    _airspeed.indicated_airspeed_m_s, eas2tas,
 					    climb_out, _parameters.pitch_limit_min,
 					    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
@@ -1181,6 +1235,7 @@ FixedwingPositionControl::task_main()
 	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
 	_manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
+	_range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder));

 	/* rate limit vehicle status updates to 5Hz */
 	orb_set_interval(_control_mode_sub, 200);
@@ -1260,6 +1315,7 @@ FixedwingPositionControl::task_main()
 			vehicle_setpoint_poll();
 			vehicle_sensor_combined_poll();
 			vehicle_airspeed_poll();
+			range_finder_poll();
 			// vehicle_baro_poll();

 			math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e);
@@ -375,3 +375,14 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f);
 * @group L1 Control
 */
 PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f);
+
+/**
+ * Relative altitude threshold for range finder measurements for use during landing
+ *
+ * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT
+ * set to < 0 to disable
+ * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f);
@@ -69,26 +69,46 @@ void Landingslope::calculateSlopeValues()
 	_horizontal_slope_displacement = (_flare_length - _d1);
 }

-float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude)
+float  Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance)
 {
-	return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
+	return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad);
 }

-float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
+float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp)
 {
 	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
 	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
-		return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude);
+		return getLandingSlopeRelativeAltitude(wp_landing_distance);
+	else
+		return 0.0f;
+}
+
+float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude)
+{
+	return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad);
+}
+
+float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude)
+{
+	/* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */
+	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
+		return getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude);
 	else
 		return wp_altitude;
 }

-float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
+float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp)
 {
 	/* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */
 	if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f))
-		return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
+		return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt;
 	else
-		return wp_landing_altitude;
+		return 0.0f;
+}
+
+float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude)
+{
+
+	return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp, bearing_airplane_currwp);
 }

@@ -63,11 +63,26 @@ public:
 	Landingslope() {}
 	~Landingslope() {}

+
+	/**
+	 *
+	 * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	float getLandingSlopeRelativeAltitude(float wp_landing_distance);
+
+	/**
+	 *
+	 * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 * Performs check if aircraft is in front of waypoint to avoid climbout
+	 */
+	float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp);
+
+
 	/**
 	 *
 	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
 	 */
-	float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude);
+	float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude);

 	/**
 	 *
@@ -76,13 +91,22 @@ public:
 	 */
 	float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude);

+	/**
+	 *
+	 * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
+	 */
+	__EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, float landing_slope_angle_rad)
+	{
+		return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); //flare_relative_alt is negative
+	}
+
 	/**
 	 *
 	 * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance
 	 */
 	__EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad)
 	{
-		return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative
+		return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, landing_slope_angle_rad) + wp_landing_altitude;
 	}

 	/**
@@ -95,8 +119,9 @@ public:

 	}

+	float getFlareCurveRelativeAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp);

-	float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);
+	float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude);

 	void update(float landing_slope_angle_rad,
 			float flare_relative_alt,
@@ -149,10 +149,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;
 	}

@@ -211,9 +208,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),
@@ -234,7 +231,6 @@ Mavlink::Mavlink() :
 	_subscribe_to_stream_rate(0.0f),
 	_flow_control_enabled(true),
 	_message_buffer({}),
-
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "mavlink"))
 {
@@ -736,9 +732,9 @@ int Mavlink::mavlink_pm_send_param(param_t param)
 	if (param == PARAM_INVALID) { return 1; }

 	/* buffers for param transmission */
-	static char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
+	char name_buf[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN];
 	float val_buf;
-	static mavlink_message_t tx_msg;
+	mavlink_message_t tx_msg;

 	/* query parameter type */
 	param_type_t type = param_type(param);
@@ -1543,6 +1539,8 @@ void Mavlink::mavlink_wpm_message_handler(const mavlink_message_t *msg)
 void
 Mavlink::mavlink_missionlib_send_message(mavlink_message_t *msg)
 {
+	uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
+
 	uint16_t len = mavlink_msg_to_send_buffer(missionlib_msg_buf, msg);

 	mavlink_send_uart_bytes(_channel, missionlib_msg_buf, len);
@@ -2039,14 +2037,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;
@@ -2213,7 +2211,7 @@ Mavlink::start(int argc, char *argv[])
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
-		       2048,
+		       1950,
 		       (main_t)&Mavlink::start_helper,
 		       (const char **)argv);

@@ -2252,7 +2250,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;
@@ -2289,7 +2286,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) {
@@ -221,8 +221,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) */
@@ -237,7 +235,6 @@ private:

 	orb_advert_t	_mission_pub;
 	struct mission_s mission;
-	uint8_t missionlib_msg_buf[MAVLINK_MAX_PACKET_LEN];
 	MAVLINK_MODE _mode;

 	uint8_t _mavlink_wpm_comp_id;
@@ -283,7 +280,7 @@ private:

 	pthread_mutex_t _message_buffer_mutex;

-
+	perf_counter_t	_loop_perf;			/**< loop performance counter */

 	/**
 	 * Send one parameter.
@@ -125,18 +125,22 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;

-		} else if (status->main_state == MAIN_STATE_SEATBELT) {
+		} else if (status->main_state == MAIN_STATE_ALTCTL) {
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTL;

-		} else if (status->main_state == MAIN_STATE_EASY) {
+		} else if (status->main_state == MAIN_STATE_POSCTL) {
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED;
-			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTL;

 		} else if (status->main_state == MAIN_STATE_AUTO) {
 			*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 {
@@ -1139,10 +1143,10 @@ protected:
 		if (manual_sub->update(t)) {
 			mavlink_msg_manual_control_send(_channel,
 							mavlink_system.sysid,
-							manual->roll * 1000,
-							manual->pitch * 1000,
-							manual->yaw * 1000,
-							manual->throttle * 1000,
+							manual->x * 1000,
+							manual->y * 1000,
+							manual->z * 1000,
+							manual->r * 1000,
 							0);
 		}
 	}
@@ -63,7 +63,7 @@ MavlinkOrbSubscription::~MavlinkOrbSubscription()
 	free(_data);
 }

-const orb_id_t
+orb_id_t
 MavlinkOrbSubscription::get_topic()
 {
 	return _topic;
@@ -63,7 +63,7 @@ public:
 	 */
 	bool is_published();
 	void *get_data();
-	const orb_id_t get_topic();
+	orb_id_t get_topic();

 private:
 	const orb_id_t _topic;		/*< topic metadata */
@@ -191,7 +191,7 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		}

 	}
-	
+
    /* If we've received a valid message, mark the flag indicating so.
 	   This is used in the '-w' command-line flag. */
 	_mavlink->set_has_received_messages(true);
@@ -438,10 +438,10 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 	memset(&manual, 0, sizeof(manual));

 	manual.timestamp = hrt_absolute_time();
-	manual.pitch = man.x / 1000.0f;
-	manual.roll = man.y / 1000.0f;
-	manual.yaw = man.r / 1000.0f;
-	manual.throttle = man.z / 1000.0f;
+	manual.x = man.x / 1000.0f;
+	manual.y = man.y / 1000.0f;
+	manual.r = man.r / 1000.0f;
+	manual.z = man.z / 1000.0f;

 	if (_manual_pub < 0) {
 		_manual_pub = orb_advertise(ORB_ID(manual_control_setpoint), &manual);
@@ -932,6 +932,8 @@ void *MavlinkReceiver::start_helper(void *context)
 	rcv->receive_thread(NULL);

 	delete rcv;
+
+	return nullptr;
 }

 pthread_t
@@ -949,7 +951,7 @@ MavlinkReceiver::receive_start(Mavlink *parent)
 	param.sched_priority = SCHED_PRIORITY_MAX - 40;
 	(void)pthread_attr_setschedparam(&receiveloop_attr, &param);

-	pthread_attr_setstacksize(&receiveloop_attr, 3000);
+	pthread_attr_setstacksize(&receiveloop_attr, 2900);

 	pthread_t thread;
 	pthread_create(&thread, &receiveloop_attr, MavlinkReceiver::start_helper, (void *)parent);
@@ -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;
 }
@@ -63,9 +63,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);
 	virtual MavlinkStream *new_instance() = 0;
 	virtual void subscribe(Mavlink *mavlink) = 0;
@@ -48,3 +48,5 @@ SRCS		 += mavlink_main.cpp \
 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink

 MAXOPTIMIZATION	 = -Os
+
+MODULE_STACKSIZE = 1024
@@ -1,9 +1,6 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *   Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
- *           @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Anton Babushkin <anton.babushkin@me.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -35,9 +32,13 @@
 ****************************************************************************/

 /**
- * @file mc_att_control_main.c
+ * @file mc_att_control_main.cpp
 * Multicopter attitude controller.
 *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ *
 * The controller has two loops: P loop for angular error and PD loop for angular rate error.
 * Desired rotation calculated keeping in mind that yaw response is normally slower than roll/pitch.
 * For small deviations controller rotates copter to have shortest path of thrust vector and independently rotates around yaw,
@@ -71,7 +72,7 @@
 #include <systemlib/err.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
-#include <mathlib/mathlib.h>
+#include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>

 /**
@@ -156,10 +157,14 @@ private:
 		param_t yaw_rate_i;
 		param_t yaw_rate_d;
 		param_t yaw_ff;
+		param_t yaw_rate_max;

 		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 {
@@ -168,10 +173,12 @@ private:
 		math::Vector<3> rate_i;				/**< I gain for angular rate error */
 		math::Vector<3> rate_d;				/**< D gain for angular rate error */
 		float yaw_ff;						/**< yaw control feed-forward */
+		float yaw_rate_max;					/**< max yaw rate */

 		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;

 	/**
@@ -225,9 +232,9 @@ private:
 	static void	task_main_trampoline(int argc, char *argv[]);

 	/**
-	 * Main sensor collection task.
+	 * Main attitude control task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 };

 namespace mc_att_control
@@ -276,6 +283,12 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.rate_p.zero();
 	_params.rate_i.zero();
 	_params.rate_d.zero();
+	_params.yaw_ff = 0.0f;
+	_params.yaw_rate_max = 0.0f;
+	_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();
@@ -298,10 +311,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.yaw_rate_i		= 	param_find("MC_YAWRATE_I");
 	_params_handles.yaw_rate_d		= 	param_find("MC_YAWRATE_D");
 	_params_handles.yaw_ff			= 	param_find("MC_YAW_FF");
-
+	_params_handles.yaw_rate_max	= 	param_find("MC_YAWRATE_MAX");
 	_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();
@@ -367,15 +383,24 @@ MulticopterAttitudeControl::parameters_update()
 	_params.rate_d(2) = v;

 	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
+	param_get(_params_handles.yaw_rate_max, &_params.yaw_rate_max);
+	_params.yaw_rate_max = math::radians(_params.yaw_rate_max);

 	/* manual control scale */
 	param_get(_params_handles.man_roll_max, &_params.man_roll_max);
 	param_get(_params_handles.man_pitch_max, &_params.man_pitch_max);
 	param_get(_params_handles.man_yaw_max, &_params.man_yaw_max);
+	_params.man_roll_max = math::radians(_params.man_roll_max);
+	_params.man_pitch_max = math::radians(_params.man_pitch_max);
+	_params.man_yaw_max = math::radians(_params.man_yaw_max);

-	_params.man_roll_max *= M_PI / 180.0;
-	_params.man_pitch_max *= M_PI / 180.0;
-	_params.man_yaw_max *= M_PI / 180.0;
+	/* 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;
 }
@@ -478,7 +503,7 @@ MulticopterAttitudeControl::control_attitude(float dt)

 		if (!_v_control_mode.flag_control_climb_rate_enabled) {
 			/* pass throttle directly if not in altitude stabilized mode */
-			_v_att_sp.thrust = _manual_control_sp.throttle;
+			_v_att_sp.thrust = _manual_control_sp.z;
 			publish_att_sp = true;
 		}

@@ -496,8 +521,16 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			//}
 		} else {
 			/* move yaw setpoint */
-			yaw_sp_move_rate = _manual_control_sp.yaw * _params.man_yaw_max;
+			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
 			_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
+			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
+			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
+			if (yaw_offs < - yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
+
+			} else if (yaw_offs > yaw_offs_max) {
+				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
+			}
 			_v_att_sp.R_valid = false;
 			publish_att_sp = true;
 		}
@@ -512,8 +545,8 @@ MulticopterAttitudeControl::control_attitude(float dt)

 		if (!_v_control_mode.flag_control_velocity_enabled) {
 			/* update attitude setpoint if not in position control mode */
-			_v_att_sp.roll_body = _manual_control_sp.roll * _params.man_roll_max;
-			_v_att_sp.pitch_body = -_manual_control_sp.pitch * _params.man_pitch_max;
+			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
+			_v_att_sp.pitch_body = -_manual_control_sp.x * _params.man_pitch_max;
 			_v_att_sp.R_valid = false;
 			publish_att_sp = true;
 		}
@@ -626,6 +659,9 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	/* calculate angular rates setpoint */
 	_rates_sp = _params.att_p.emult(e_R);

+	/* limit yaw rate */
+	_rates_sp(2) = math::constrain(_rates_sp(2), -_params.yaw_rate_max, _params.yaw_rate_max);
+
 	/* feed forward yaw setpoint rate */
 	_rates_sp(2) += yaw_sp_move_rate * yaw_w * _params.yaw_ff;
 }
@@ -762,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) {
@@ -806,7 +867,7 @@ MulticopterAttitudeControl::start()
 	_control_task = task_spawn_cmd("mc_att_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2048,
+				       2000,
 				       (main_t)&MulticopterAttitudeControl::task_main_trampoline,
 				       nullptr);

@@ -1,9 +1,6 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *   Author: @author Tobias Naegeli <naegelit@student.ethz.ch>
- *   	     @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Anton Babushkin <anton.babushkin@me.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -37,6 +34,10 @@
 /**
 * @file mc_att_control_params.c
 * Parameters for multicopter attitude controller.
+ *
+ * @author Tobias Naegeli <naegelit@student.ethz.ch>
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Anton Babushkin <anton.babushkin@me.com>
 */

 #include <systemlib/param/param.h>
@@ -174,6 +175,18 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
 */
 PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);

+/**
+ * Max yaw rate
+ *
+ * Limit for yaw rate, has effect for large rotations in autonomous mode, to avoid large control output and mixer saturation.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 360.0
+ * @group Multicopter Attitude Control
+ */
+PARAM_DEFINE_FLOAT(MC_YAWRATE_MAX, 120.0f);
+
 /**
 * Max manual roll
 *
@@ -202,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);
@@ -226,7 +226,7 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 };

 namespace pos_control
@@ -617,7 +617,7 @@ MulticopterPositionControl::task_main()
 					reset_alt_sp();

 					/* move altitude setpoint with throttle stick */
-					sp_move_rate(2) = -scale_control(_manual.throttle - 0.5f, 0.5f, alt_ctl_dz);
+					sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
 				}

 				if (_control_mode.flag_control_position_enabled) {
@@ -625,8 +625,8 @@ MulticopterPositionControl::task_main()
 					reset_pos_sp();

 					/* move position setpoint with roll/pitch stick */
-					sp_move_rate(0) = _manual.pitch;
-					sp_move_rate(1) = _manual.roll;
+					sp_move_rate(0) = _manual.x;
+					sp_move_rate(1) = _manual.y;
 				}

 				/* limit setpoint move rate */
@@ -782,7 +782,7 @@ MulticopterPositionControl::task_main()
 							float i = _params.thr_min;

 							if (reset_int_z_manual) {
-								i = _manual.throttle;
+								i = _manual.z;

 								if (i < _params.thr_min) {
 									i = _params.thr_min;
@@ -1062,7 +1062,7 @@ MulticopterPositionControl::start()
 	_control_task = task_spawn_cmd("mc_pos_control",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2048,
+				       2000,
 				       (main_t)&MulticopterPositionControl::task_main_trampoline,
 				       nullptr);

@@ -35,6 +35,8 @@
 /**
 * @file mc_pos_control_params.c
 * Multicopter position controller parameters.
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
 */

 #include <systemlib/param/param.h>
@@ -98,7 +100,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f);
 /**
 * Maximum vertical velocity
 *
- * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY).
+ * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL).
 *
 * @unit m/s
 * @min 0.0
@@ -109,7 +111,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f);
 /**
 * Vertical velocity feed forward
 *
- * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ * Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
 *
 * @min 0.0
 * @max 1.0
@@ -154,7 +156,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f);
 /**
 * Maximum horizontal velocity
 *
- * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY).
+ * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL).
 *
 * @unit m/s
 * @min 0.0
@@ -165,7 +167,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f);
 /**
 * Horizontal velocity feed forward
 *
- * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
+ * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot.
 *
 * @min 0.0
 * @max 1.0
@@ -176,7 +178,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f);
 /**
 * Maximum tilt angle in air
 *
- * Limits maximum tilt in AUTO and EASY modes during flight.
+ * Limits maximum tilt in AUTO and POSCTRL modes during flight.
 *
 * @unit deg
 * @min 0.0
@@ -1,8 +1,6 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Jean Cyr <jean.m.cyr@gmail.com>
- *           @author Thomas Gubler <thomasgubler@gmail.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -35,6 +33,9 @@
 /**
 * @file geofence.cpp
 * Provides functions for handling the geofence
+ *
+ * @author Jean Cyr <jean.m.cyr@gmail.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
 */
 #include "geofence.h"

@@ -1,8 +1,6 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Jean Cyr <jean.m.cyr@gmail.com>
- *           @author Thomas Gubler <thomasgubler@gmail.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -35,6 +33,9 @@
 /**
 * @file geofence.h
 * Provides functions for handling the geofence
+ *
+ * @author Jean Cyr <jean.m.cyr@gmail.com>
+ * @author Thomas Gubler <thomasgubler@gmail.com>
 */

 #ifndef GEOFENCE_H_
@@ -139,7 +139,7 @@ Mission::get_current_mission_item(struct mission_item_s *new_mission_item, bool
 			return true;
 		}
 	}
-	
+
 	/* otherwise fallback to offboard */
 	if (_current_offboard_mission_index < _offboard_mission_item_count) {

@@ -246,7 +246,7 @@ void
 Mission::move_to_next()
 {
 	report_mission_item_reached();
-	
+
 	switch (_current_mission_type) {
 	case MISSION_TYPE_ONBOARD:
 		_current_onboard_mission_index++;
@@ -34,6 +34,8 @@
 * @file navigator_mission.h
 * Helper class to access missions
 * @author Julian Oes <julian@oes.ch>
+ *
+ * @author Julian Oes <joes@student.ethz.ch>
 */

 #ifndef NAVIGATOR_MISSION_H
@@ -1,8 +1,6 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -35,6 +33,9 @@
 /**
 * @file mission_feasibility_checker.cpp
 * Provides checks if mission is feasible given the navigation capabilities
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
 */

 #include "mission_feasibility_checker.h"
@@ -100,8 +101,8 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 	/* Check if all mission items are inside the geofence (if we have a valid geofence) */
 	if (geofence.valid()) {
 		for (size_t i = 0; i < nMissionItems; i++) {
-			static struct mission_item_s missionitem;
-			const ssize_t len = sizeof(struct mission_item_s);
+			struct mission_item_s missionitem;
+			const ssize_t len = sizeof(missionitem);

 			if (dm_read(dm_current, i, &missionitem, len) != len) {
 				/* not supposed to happen unless the datamanager can't access the SD card, etc. */
@@ -125,8 +126,8 @@ bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size


 	for (size_t i = 0; i < nMissionItems; i++) {
-		static struct mission_item_s missionitem;
-		const ssize_t len = sizeof(struct mission_item_s);
+		struct mission_item_s missionitem;
+		const ssize_t len = sizeof(missionitem);
 		if (dm_read(dm_current, i, &missionitem, len) != len) {
 			/* not supposed to happen unless the datamanager can't access the SD card, etc. */
 			return false;
@@ -1,8 +1,6 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Thomas Gubler <thomasgubler@student.ethz.ch>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -35,7 +33,11 @@
 /**
 * @file mission_feasibility_checker.h
 * Provides checks if mission is feasible given the navigation capabilities
+ *
+ * @author Lorenz Meier <lm@inf.ethz.ch>
+ * @author Thomas Gubler <thomasgubler@student.ethz.ch>
 */
+
 #ifndef MISSION_FEASIBILITY_CHECKER_H_
 #define MISSION_FEASIBILITY_CHECKER_H_

@@ -47,3 +47,5 @@ SRCS		= navigator_main.cpp \
 		  geofence_params.c

 INCLUDE_DIRS	 += $(MAVLINK_SRC)/include/mavlink
+
+MODULE_STACKSIZE = 1200
@@ -1,6 +1,6 @@
 /****************************************************************************
 *
- *   Copyright (c) 2013-2014 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -410,7 +410,7 @@ Navigator::parameters_update()
 	param_get(_parameter_handles.acceptance_radius, &(_parameters.acceptance_radius));
 	param_get(_parameter_handles.loiter_radius, &(_parameters.loiter_radius));
 	param_get(_parameter_handles.onboard_mission_enabled, &(_parameters.onboard_mission_enabled));
-	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));	
+	param_get(_parameter_handles.takeoff_alt, &(_parameters.takeoff_alt));

 	_mission.set_onboard_mission_allowed((bool)_parameters.onboard_mission_enabled);

@@ -728,7 +728,7 @@ Navigator::start()
 	_navigator_task = task_spawn_cmd("navigator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 5,
-					 2048,
+					 2000,
 					 (main_t)&Navigator::task_main_trampoline,
 					 nullptr);

@@ -946,7 +946,7 @@ Navigator::start_loiter()
 	mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");

 	_update_triplet = true;
-	return true;	
+	return true;
 }

 bool
@@ -999,7 +999,7 @@ Navigator::set_mission_items()
 	/* convert the current mission item and set it valid */
 	mission_item_to_position_setpoint(&_mission_item, &_pos_sp_triplet.current);
 	_pos_sp_triplet.current.valid = true;
-	
+

 	mission_item_s next_mission_item;

@@ -1206,7 +1206,7 @@ Navigator::check_mission_item_reached()
 	}

 	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
-		
+
 		/* TODO: removed takeoff, why? */
 		if (_vstatus.is_rotary_wing && isfinite(_mission_item.yaw)) {

@@ -1,9 +1,6 @@
 /****************************************************************************
 *
 *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
- *   Author: @author Lorenz Meier <lm@inf.ethz.ch>
- *           @author Julian Oes <joes@student.ethz.ch>
- *           @author Anton Babushkin <anton.babushkin@me.com>
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
@@ -39,3 +39,5 @@ MODULE_COMMAND	 	= position_estimator_inav
 SRCS		 	= position_estimator_inav_main.c \
 			position_estimator_inav_params.c \
 			inertial_filter.c
+
+MODULE_STACKSIZE = 1200
@@ -135,7 +135,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_RR, SCHED_PRIORITY_MAX - 5, 4096,
+					       SCHED_DEFAULT, SCHED_PRIORITY_MAX - 5, 4000,
 					       position_estimator_inav_thread_main,
 					       (argv) ? (const char **) &argv[2] : (const char **) NULL);
 		exit(0);
@@ -199,6 +199,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	float y_est[3] = { 0.0f, 0.0f, 0.0f };
 	float z_est[3] = { 0.0f, 0.0f, 0.0f };

+	float eph = 1.0;
+	float epv = 1.0;
+
 	float x_est_prev[3], y_est_prev[3], z_est_prev[3];
 	memset(x_est_prev, 0, sizeof(x_est_prev));
 	memset(y_est_prev, 0, sizeof(y_est_prev));
@@ -535,6 +538,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])

 					flow_valid = true;

+					eph = fminf(eph, 0.1 / att.R[2][2] / flow_q * fmaxf(1.0f, flow_dist));	// under ideal conditions, on 1m distance assume EPH = 10cm
+
 				} else {
 					w_flow = 0.0f;
 					flow_valid = false;
@@ -673,6 +678,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 							corr_gps[2][1] = 0.0f;
 						}

+						eph = fminf(eph, gps.eph);
+						epv = fminf(epv, gps.epv);
+
 						w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph);
 						w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv);
 					}
@@ -712,6 +720,10 @@ 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 */
+		eph *= 1.0 + dt;
+		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 +735,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 * 1.5;

 		bool dist_bottom_valid = (t < sonar_valid_time + sonar_valid_timeout);

@@ -922,6 +934,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 +964,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;
-				global_pos.epv = gps.epv;
+				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);
@@ -291,6 +291,7 @@ controls_tick() {

 		/* set RC OK flag, as we got an update */
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_OK;
+		r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_OK;

 		/* if we have enough channels (5) to control the vehicle, the mapping is ok */
 		if (assigned_channels > 4) {
@@ -336,6 +337,9 @@ controls_tick() {
 			PX4IO_P_STATUS_FLAGS_OVERRIDE |
 			PX4IO_P_STATUS_FLAGS_RC_OK);

+		/* flag raw RC as lost */
+		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
+
 		/* Mark all channels as invalid, as we just lost the RX */
 		r_rc_valid = 0;

@@ -405,8 +409,9 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 		if (*num_values > PX4IO_RC_INPUT_CHANNELS)
 			*num_values = PX4IO_RC_INPUT_CHANNELS;

-		for (unsigned i = 0; i < *num_values; i++)
+		for (unsigned i = 0; i < *num_values; i++) {
 			values[i] = ppm_buffer[i];
+		}

 		/* clear validity */
 		ppm_last_valid_decode = 0;
@@ -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++)
@@ -142,6 +142,7 @@
 #define PX4IO_P_RAW_RC_FLAGS_FAILSAFE		(1 << 1) /* receiver is in failsafe mode */
 #define PX4IO_P_RAW_RC_FLAGS_RC_DSM11		(1 << 2) /* DSM decoding is 11 bit mode */
 #define PX4IO_P_RAW_RC_FLAGS_MAPPING_OK		(1 << 3) /* Channel mapping is ok */
+#define PX4IO_P_RAW_RC_FLAGS_RC_OK		(1 << 4) /* RC reception ok */

 #define PX4IO_P_RAW_RC_NRSSI			2	/* [2] Normalized RSSI value, 0: no reception, 255: perfect reception */
 #define PX4IO_P_RAW_RC_DATA			3	/* [1] + [2] Details about the RC source (PPM frame length, Spektrum protocol type) */
@@ -189,6 +190,8 @@
 #define PX4IO_P_SETUP_RELAYS_POWER2		(1<<1)	/* hardware rev [1] power relay 2 */
 #define PX4IO_P_SETUP_RELAYS_ACC1		(1<<2)	/* hardware rev [1] accessory power 1 */
 #define PX4IO_P_SETUP_RELAYS_ACC2		(1<<3)	/* hardware rev [1] accessory power 2 */
+#else
+#define PX4IO_P_SETUP_RELAYS_PAD		5
 #endif

 #define PX4IO_P_SETUP_VBATT_SCALE		6	/* hardware rev [1] battery voltage correction factor (float) */
@@ -208,15 +211,16 @@ enum {							/* DSM bind states */
 #define PX4IO_REBOOT_BL_MAGIC			14662	/* required argument for reboot (random) */

 #define PX4IO_P_SETUP_CRC			11	/* get CRC of IO firmware */
-						/* 12 occupied by CRC */
+						/* storage space of 12 occupied by CRC */
+#define PX4IO_P_SETUP_FORCE_SAFETY_OFF		12	/* force safety switch into
+                                                           'armed' (PWM enabled) state - this is a non-data write and
+                                                           hence index 12 can safely be used. */
 #define PX4IO_P_SETUP_RC_THR_FAILSAFE_US	13	/**< the throttle failsafe pulse length in microseconds */

-#define PX4IO_P_SETUP_FORCE_SAFETY_OFF	       12	/* force safety switch into
-                                                           'armed' (PWM enabled) state */
-#define PX4IO_FORCE_SAFETY_MAGIC            22027       /* required argument for force safety (random) */
+#define PX4IO_FORCE_SAFETY_MAGIC		22027	/* required argument for force safety (random) */

 /* autopilot control values, -10000..10000 */
-#define PX4IO_PAGE_CONTROLS			51		/**< actuator control groups, one after the other, 8 wide */
+#define PX4IO_PAGE_CONTROLS			51	/**< actuator control groups, one after the other, 8 wide */
 #define PX4IO_P_CONTROLS_GROUP_0		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 0)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
 #define PX4IO_P_CONTROLS_GROUP_1		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 1)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
 #define PX4IO_P_CONTROLS_GROUP_2		(PX4IO_PROTOCOL_MAX_CONTROL_COUNT * 2)	/**< 0..PX4IO_PROTOCOL_MAX_CONTROL_COUNT - 1 */
@@ -160,6 +160,9 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_PWM_ALTRATE]		= 200,
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 	[PX4IO_P_SETUP_RELAYS]			= 0,
+#else
+	/* this is unused, but we will pad it for readability (the compiler pads it automatically) */
+	[PX4IO_P_SETUP_RELAYS_PAD]		= 0,
 #endif
 #ifdef ADC_VSERVO
 	[PX4IO_P_SETUP_VSERVO_SCALE]		= 10000,
@@ -523,18 +526,22 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			break;

 		case PX4IO_P_SETUP_PWM_DEFAULTRATE:
-			if (value < 50)
+			if (value < 50) {
 				value = 50;
-			if (value > 400)
+			}
+			if (value > 400) {
 				value = 400;
+			}
 			pwm_configure_rates(r_setup_pwm_rates, value, r_setup_pwm_altrate);
 			break;

 		case PX4IO_P_SETUP_PWM_ALTRATE:
-			if (value < 50)
+			if (value < 50) {
 				value = 50;
-			if (value > 400)
+			}
+			if (value > 400) {
 				value = 400;
+			}
 			pwm_configure_rates(r_setup_pwm_rates, r_setup_pwm_defaultrate, value);
 			break;

@@ -566,8 +573,9 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			}

 			// check the magic value
-			if (value != PX4IO_REBOOT_BL_MAGIC)
+			if (value != PX4IO_REBOOT_BL_MAGIC) {
 				break;
+			}

                        // we schedule a reboot rather than rebooting
                        // immediately to allow the IO board to ACK
@@ -585,6 +593,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			}
 			break;

+		case PX4IO_P_SETUP_RC_THR_FAILSAFE_US:
+			if (value > 650 && value < 2350) {
+				r_page_setup[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = value;
+			}
+			break;
+
 		default:
 			return -1;
 		}
@@ -41,3 +41,5 @@ MODULE_PRIORITY = "SCHED_PRIORITY_MAX-30"

 SRCS = sdlog2.c \
       logbuffer.c
+
+MODULE_STACKSIZE = 1200
@@ -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,12 +778,12 @@ 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);

-				if (r <= 0) {
+				if (r == 0) {
 					r = 1;
 				}

@@ -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,6 +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_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)
@@ -967,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;
@@ -981,7 +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);
+
+			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;
+
+					/* 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);
+			}
 		}

 		/* --- SENSOR COMBINED --- */
@@ -1105,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);
 		}

@@ -1320,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);
 }

@@ -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,6 +322,30 @@ struct log_VICN_s {
 	float yaw;
 };

+/* --- 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 */
+};
+
+/* --- 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 **********/

 /* --- TIME - TIME STAMP --- */
@@ -348,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"),
@@ -368,6 +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(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 */
@@ -35,8 +35,8 @@ void BlockSegwayController::update() {

 	// handle autopilot modes
 	if (_status.main_state == MAIN_STATE_AUTO ||
-	    _status.main_state == MAIN_STATE_SEATBELT ||
-	    _status.main_state == MAIN_STATE_EASY) {
+	    _status.main_state == MAIN_STATE_ALTCTL ||
+	    _status.main_state == MAIN_STATE_POSCTL) {
 		_actuators.control[0] = spdCmd;
 		_actuators.control[1] = spdCmd;

@@ -40,3 +40,5 @@ MODULE_PRIORITY	= "SCHED_PRIORITY_MAX-5"

 SRCS		= sensors.cpp \
 		  sensor_params.c
+
+MODULE_STACKSIZE = 1200
@@ -488,6 +488,15 @@ PARAM_DEFINE_INT32(BAT_V_SCALE_IO, 10000);
 * @group Battery Calibration
 */
 PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0082f);
+#elif CONFIG_ARCH_BOARD_AEROCORE
+/**
+ * Scaling factor for battery voltage sensor on AeroCore.
+ *
+ * For R70 = 133K, R71 = 10K --> scale = 1.8 * 143 / (4096*10) = 0.0063
+ *
+ * @group Battery Calibration
+ */
+PARAM_DEFINE_FLOAT(BAT_V_SCALING, 0.0063f);
 #else
 /**
 * Scaling factor for battery voltage sensor on FMU v1.
@@ -535,6 +544,20 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1);
 */
 PARAM_DEFINE_INT32(RC_MAP_PITCH, 2);

+/**
+ * Failsafe channel mapping.
+ *
+ * The RC mapping index indicates which channel is used for failsafe
+ * If 0, whichever channel is mapped to throttle is used
+ * otherwise the value indicates the specific rc channel to use
+ *
+ * @min 0
+ * @max 18
+ *
+ *
+ */
+PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0);  //Default to throttle function
+
 /**
 * Throttle control channel mapping.
 *
@@ -585,13 +608,13 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);

 /**
- * Assist switch channel mapping.
+ * Posctl switch channel mapping.
 *
 * @min 0
 * @max 18
 * @group Radio Calibration
 */
-PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0);

 /**
 * Loiter switch channel mapping.
@@ -602,7 +625,14 @@ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
 */
 PARAM_DEFINE_INT32(RC_MAP_LOITER_SW, 0);

-//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
+/**
+ * Acro switch channel mapping.
+ *
+ * @min 0
+ * @max 18
+ * @group Radio Calibration
+ */
+PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);

 /**
 * Flaps channel mapping.
@@ -655,3 +685,99 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0);
 * @group Radio Calibration
 */
 PARAM_DEFINE_INT32(RC_FAILS_THR, 0);
+
+/**
+ * Threshold for selecting assist 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_ASSIST_TH, 0.25f);
+
+/**
+ * Threshold for selecting auto 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_AUTO_TH, 0.75f);
+
+/**
+ * Threshold for selecting posctl 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_POSCTL_TH, 0.5f);
+
+/**
+ * Threshold for selecting return to launch 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_RETURN_TH, 0.5f);
+
+/**
+ * Threshold for selecting loiter 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_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);
@@ -126,6 +126,12 @@
 #define ADC_AIRSPEED_VOLTAGE_CHANNEL	15
 #endif

+#ifdef CONFIG_ARCH_BOARD_AEROCORE
+#define ADC_BATTERY_VOLTAGE_CHANNEL	10
+#define ADC_BATTERY_CURRENT_CHANNEL	-1
+#define ADC_AIRSPEED_VOLTAGE_CHANNEL	-1
+#endif
+
 #define BATT_V_LOWPASS 0.001f
 #define BATT_V_IGNORE_THRESHOLD 3.5f

@@ -175,7 +181,8 @@ private:
 	/**
 	 * Get switch position for specified function.
 	 */
-	switch_pos_t	get_rc_switch_position(enum RC_CHANNELS_FUNCTION func);
+	switch_pos_t	get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv);
+	switch_pos_t	get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv);

 	/**
 	 * Gather and publish RC input data.
@@ -219,8 +226,8 @@ private:
 	struct differential_pressure_s _diff_pres;
 	struct airspeed_s _airspeed;

-	math::Matrix<3,3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
-	math::Matrix<3,3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
+	math::Matrix<3, 3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
+	math::Matrix<3, 3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */

 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
@@ -250,13 +257,13 @@ private:
 		int rc_map_pitch;
 		int rc_map_yaw;
 		int rc_map_throttle;
+		int rc_map_failsafe;

 		int rc_map_mode_sw;
 		int rc_map_return_sw;
-		int rc_map_assisted_sw;
+		int rc_map_posctl_sw;
 		int rc_map_loiter_sw;
-
-//		int rc_map_offboard_ctrl_mode_sw;
+		int rc_map_acro_sw;

 		int rc_map_flaps;

@@ -266,7 +273,19 @@ private:
 		int rc_map_aux4;
 		int rc_map_aux5;

-		int32_t rc_fs_thr;
+		int32_t rc_fails_thr;
+		float rc_assist_th;
+		float rc_auto_th;
+		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;
@@ -293,13 +312,13 @@ private:
 		param_t rc_map_pitch;
 		param_t rc_map_yaw;
 		param_t rc_map_throttle;
+		param_t rc_map_failsafe;

 		param_t rc_map_mode_sw;
 		param_t rc_map_return_sw;
-		param_t rc_map_assisted_sw;
+		param_t rc_map_posctl_sw;
 		param_t rc_map_loiter_sw;
-
-//		param_t rc_map_offboard_ctrl_mode_sw;
+		param_t rc_map_acro_sw;

 		param_t rc_map_flaps;

@@ -309,7 +328,13 @@ private:
 		param_t rc_map_aux4;
 		param_t rc_map_aux5;

-		param_t rc_fs_thr;
+		param_t rc_fails_thr;
+		param_t rc_assist_th;
+		param_t rc_auto_th;
+		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;
@@ -416,7 +441,7 @@ private:
 	/**
 	 * Main sensor collection task.
 	 */
-	void		task_main() __attribute__((noreturn));
+	void		task_main();
 };

 namespace sensors
@@ -499,6 +524,7 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH");
 	_parameter_handles.rc_map_yaw 	= param_find("RC_MAP_YAW");
 	_parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE");
+	_parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE");

 	/* mandatory mode switches, mapped to channel 5 and 6 per default */
 	_parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW");
@@ -507,10 +533,9 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS");

 	/* optional mode switches, not mapped per default */
-	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
+	_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_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_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");
@@ -518,8 +543,14 @@ Sensors::Sensors() :
 	_parameter_handles.rc_map_aux4 = param_find("RC_MAP_AUX4");
 	_parameter_handles.rc_map_aux5 = param_find("RC_MAP_AUX5");

-	/* RC failsafe */
-	_parameter_handles.rc_fs_thr = param_find("RC_FAILS_THR");
+	/* RC thresholds */
+	_parameter_handles.rc_fails_thr = param_find("RC_FAILS_THR");
+	_parameter_handles.rc_assist_th = param_find("RC_ASSIST_TH");
+	_parameter_handles.rc_auto_th = param_find("RC_AUTO_TH");
+	_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");
@@ -620,8 +651,9 @@ Sensors::parameters_update()
 	}

 	/* handle wrong values */
-	if (!rc_valid)
+	if (!rc_valid) {
 		warnx("WARNING     WARNING     WARNING\n\nRC CALIBRATION NOT SANE!\n\n");
+	}

 	const char *paramerr = "FAIL PARM LOAD";

@@ -642,6 +674,10 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}

+	if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) {
+		warnx("%s", paramerr);
+	}
+
 	if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) {
 		warnx("%s", paramerr);
 	}
@@ -650,7 +686,7 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}

-	if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) {
+	if (param_get(_parameter_handles.rc_map_posctl_sw, &(_parameters.rc_map_posctl_sw)) != OK) {
 		warnx("%s", paramerr);
 	}

@@ -658,20 +694,38 @@ Sensors::parameters_update()
 		warnx("%s", paramerr);
 	}

-	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
+	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_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) {
-//		warnx("Failed getting offboard control mode sw chan index");
-//	}
+	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
+		warnx("%s", paramerr);
+	}

 	param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1));
 	param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2));
 	param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3));
 	param_get(_parameter_handles.rc_map_aux4, &(_parameters.rc_map_aux4));
 	param_get(_parameter_handles.rc_map_aux5, &(_parameters.rc_map_aux5));
-	param_get(_parameter_handles.rc_fs_thr, &(_parameters.rc_fs_thr));
+	param_get(_parameter_handles.rc_fails_thr, &(_parameters.rc_fails_thr));
+	param_get(_parameter_handles.rc_assist_th, &(_parameters.rc_assist_th));
+	_parameters.rc_assist_inv = (_parameters.rc_assist_th < 0);
+	_parameters.rc_assist_th = fabs(_parameters.rc_assist_th);
+	param_get(_parameter_handles.rc_auto_th, &(_parameters.rc_auto_th));
+	_parameters.rc_auto_inv = (_parameters.rc_auto_th < 0);
+	_parameters.rc_auto_th = fabs(_parameters.rc_auto_th);
+	param_get(_parameter_handles.rc_posctl_th, &(_parameters.rc_posctl_th));
+	_parameters.rc_posctl_inv = (_parameters.rc_posctl_th < 0);
+	_parameters.rc_posctl_th = fabs(_parameters.rc_posctl_th);
+	param_get(_parameter_handles.rc_return_th, &(_parameters.rc_return_th));
+	_parameters.rc_return_inv = (_parameters.rc_return_th < 0);
+	_parameters.rc_return_th = fabs(_parameters.rc_return_th);
+	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;
@@ -681,13 +735,12 @@ Sensors::parameters_update()

 	_rc.function[MODE] = _parameters.rc_map_mode_sw - 1;
 	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
-	_rc.function[ASSISTED] = _parameters.rc_map_assisted_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;

-//	_rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1;
-
 	_rc.function[AUX_1] = _parameters.rc_map_aux1 - 1;
 	_rc.function[AUX_2] = _parameters.rc_map_aux2 - 1;
 	_rc.function[AUX_3] = _parameters.rc_map_aux3 - 1;
@@ -765,7 +818,7 @@ Sensors::accel_init()
 		/* set the driver to poll at 1000Hz */
 		ioctl(fd, SENSORIOCSPOLLRATE, 1000);

-#elif CONFIG_ARCH_BOARD_PX4FMU_V2
+#elif CONFIG_ARCH_BOARD_PX4FMU_V2 || CONFIG_ARCH_BOARD_AEROCORE

 		/* set the accel internal sampling rate up to at leat 800Hz */
 		ioctl(fd, ACCELIOCSSAMPLERATE, 800);
@@ -774,7 +827,7 @@ Sensors::accel_init()
 		ioctl(fd, SENSORIOCSPOLLRATE, 800);

 #else
-#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1 or CONFIG_ARCH_BOARD_PX4FMU_V2
+#error Need a board configuration, either CONFIG_ARCH_BOARD_PX4FMU_V1, CONFIG_ARCH_BOARD_PX4FMU_V2 or CONFIG_ARCH_BOARD_AEROCORE

 #endif

@@ -800,12 +853,14 @@ Sensors::gyro_init()
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1

 		/* set the gyro internal sampling rate up to at least 1000Hz */
-		if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK)
+		if (ioctl(fd, GYROIOCSSAMPLERATE, 1000) != OK) {
 			ioctl(fd, GYROIOCSSAMPLERATE, 800);
+		}

 		/* set the driver to poll at 1000Hz */
-		if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK)
+		if (ioctl(fd, SENSORIOCSPOLLRATE, 1000) != OK) {
 			ioctl(fd, SENSORIOCSPOLLRATE, 800);
+		}

 #else

@@ -860,12 +915,15 @@ Sensors::mag_init()

 	ret = ioctl(fd, MAGIOCGEXTERNAL, 0);

-	if (ret < 0)
+	if (ret < 0) {
 		errx(1, "FATAL: unknown if magnetometer is external or onboard");
-	else if (ret == 1)
+
+	} else if (ret == 1) {
 		_mag_is_external = true;
-	else
+
+	} else {
 		_mag_is_external = false;
+	}

 	close(fd);
 }
@@ -965,10 +1023,12 @@ Sensors::mag_poll(struct sensor_combined_s &raw)

 		math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z);

-		if (_mag_is_external)
+		if (_mag_is_external) {
 			vect = _external_mag_rotation * vect;
-		else
+
+		} else {
 			vect = _board_rotation * vect;
+		}

 		raw.magnetometer_ga[0] = vect(0);
 		raw.magnetometer_ga[1] = vect(1);
@@ -1086,8 +1146,9 @@ Sensors::parameter_update_poll(bool forced)
 			_parameters.gyro_scale[2],
 		};

-		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale))
+		if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) {
 			warn("WARNING: failed to set scale / offsets for gyro");
+		}

 		close(fd);

@@ -1101,8 +1162,9 @@ Sensors::parameter_update_poll(bool forced)
 			_parameters.accel_scale[2],
 		};

-		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale))
+		if (OK != ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&ascale)) {
 			warn("WARNING: failed to set scale / offsets for accel");
+		}

 		close(fd);

@@ -1116,8 +1178,9 @@ Sensors::parameter_update_poll(bool forced)
 			_parameters.mag_scale[2],
 		};

-		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale))
+		if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) {
 			warn("WARNING: failed to set scale / offsets for mag");
+		}

 		close(fd);

@@ -1131,8 +1194,10 @@ Sensors::parameter_update_poll(bool forced)
 				1.0f,
 			};

-			if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
+			if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
 				warn("WARNING: failed to set scale / offsets for airspeed sensor");
+			}
+
 			close(fd);
 		}

@@ -1150,10 +1215,12 @@ void
 Sensors::adc_poll(struct sensor_combined_s &raw)
 {
 	/* only read if publishing */
-	if (!_publishing)
+	if (!_publishing) {
 		return;
+	}

 	hrt_abstime t = hrt_absolute_time();
+
 	/* rate limit to 100 Hz */
 	if (t - _last_adc >= 10000) {
 		/* make space for a maximum of twelve channels (to ensure reading all channels at once) */
@@ -1178,6 +1245,7 @@ Sensors::adc_poll(struct sensor_combined_s &raw)

 					if (voltage > BATT_V_IGNORE_THRESHOLD) {
 						_battery_status.voltage_v = voltage;
+
 						/* one-time initialization of low-pass value to avoid long init delays */
 						if (_battery_status.voltage_filtered_v < BATT_V_IGNORE_THRESHOLD) {
 							_battery_status.voltage_filtered_v = voltage;
@@ -1196,19 +1264,24 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					/* handle current only if voltage is valid */
 					if (_battery_status.voltage_v > 0.0f) {
 						float current = (buf_adc[i].am_data * _parameters.battery_current_scaling);
+
 						/* check measured current value */
 						if (current >= 0.0f) {
 							_battery_status.timestamp = t;
 							_battery_status.current_a = current;
+
 							if (_battery_current_timestamp != 0) {
 								/* initialize discharged value */
-								if (_battery_status.discharged_mah < 0.0f)
+								if (_battery_status.discharged_mah < 0.0f) {
 									_battery_status.discharged_mah = 0.0f;
+								}
+
 								_battery_discharged += current * (t - _battery_current_timestamp);
 								_battery_status.discharged_mah = ((float) _battery_discharged) / 3600000.0f;
 							}
 						}
 					}
+
 					_battery_current_timestamp = t;

 				} else if (ADC_AIRSPEED_VOLTAGE_CHANNEL == buf_adc[i].am_channel) {
@@ -1241,7 +1314,9 @@ Sensors::adc_poll(struct sensor_combined_s &raw)
 					}
 				}
 			}
+
 			_last_adc = t;
+
 			if (_battery_status.voltage_filtered_v > BATT_V_IGNORE_THRESHOLD) {
 				/* announce the battery status if needed, just publish else */
 				if (_battery_pub > 0) {
@@ -1260,6 +1335,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
 {
 	if (_rc.function[func] >= 0) {
 		float value = _rc.chan[_rc.function[func]].scaled;
+
 		if (value < min_value) {
 			return min_value;

@@ -1269,24 +1345,44 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max
 		} else {
 			return value;
 		}
+
 	} else {
 		return 0.0f;
 	}
 }

 switch_pos_t
-Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func)
+Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv, float mid_th, bool mid_inv)
 {
 	if (_rc.function[func] >= 0) {
-		float value = _rc.chan[_rc.function[func]].scaled;
-		if (value > STICK_ON_OFF_LIMIT) {
+		float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f;
+
+		if (on_inv ? value < on_th : value > on_th) {
 			return SWITCH_POS_ON;

-		} else if (value < -STICK_ON_OFF_LIMIT) {
-			return SWITCH_POS_OFF;
+		} else if (mid_inv ? value < mid_th : value > mid_th) {
+			return SWITCH_POS_MIDDLE;

 		} else {
-			return SWITCH_POS_MIDDLE;
+			return SWITCH_POS_OFF;
+		}
+
+	} else {
+		return SWITCH_POS_NONE;
+	}
+}
+
+switch_pos_t
+Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv)
+{
+	if (_rc.function[func] >= 0) {
+		float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f;
+
+		if (on_inv ? value < on_th : value > on_th) {
+			return SWITCH_POS_ON;
+
+		} else {
+			return SWITCH_POS_OFF;
 		}

 	} else {
@@ -1318,13 +1414,18 @@ Sensors::rc_poll()
 			/* signal looks good */
 			signal_lost = false;

-			/* check throttle failsafe */
-			int8_t thr_ch = _rc.function[THROTTLE];
-			if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) {
-				/* throttle failsafe configured */
-				if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) ||
-					(_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) {
-					/* throttle failsafe triggered, signal is lost by receiver */
+			/* check failsafe */
+			int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle
+
+			if (_parameters.rc_map_failsafe > 0) { // if not 0, use channel number instead of rc.function mapping
+				fs_ch = _parameters.rc_map_failsafe - 1;
+			}
+
+			if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) {
+				/* failsafe configured */
+				if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) ||
+				    (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) {
+					/* failsafe triggered, signal is lost by receiver */
 					signal_lost = true;
 				}
 			}
@@ -1332,8 +1433,9 @@ Sensors::rc_poll()

 		unsigned channel_limit = rc_input.channel_count;

-		if (channel_limit > _rc_max_chan_count)
+		if (channel_limit > _rc_max_chan_count) {
 			channel_limit = _rc_max_chan_count;
+		}

 		/* read out and scale values from raw message even if signal is invalid */
 		for (unsigned int i = 0; i < channel_limit; i++) {
@@ -1341,11 +1443,13 @@ Sensors::rc_poll()
 			/*
 			 * 1) Constrain to min/max values, as later processing depends on bounds.
 			 */
-			if (rc_input.values[i] < _parameters.min[i])
+			if (rc_input.values[i] < _parameters.min[i]) {
 				rc_input.values[i] = _parameters.min[i];
+			}

-			if (rc_input.values[i] > _parameters.max[i])
+			if (rc_input.values[i] > _parameters.max[i]) {
 				rc_input.values[i] = _parameters.max[i];
+			}

 			/*
 			 * 2) Scale around the mid point differently for lower and upper range.
@@ -1377,8 +1481,9 @@ Sensors::rc_poll()
 			_rc.chan[i].scaled *= _parameters.rev[i];

 			/* handle any parameter-induced blowups */
-			if (!isfinite(_rc.chan[i].scaled))
+			if (!isfinite(_rc.chan[i].scaled)) {
 				_rc.chan[i].scaled = 0.0f;
+			}
 		}

 		_rc.chan_count = rc_input.channel_count;
@@ -1402,10 +1507,10 @@ Sensors::rc_poll()
 			manual.timestamp = rc_input.timestamp_last_signal;

 			/* limit controls */
-			manual.roll = get_rc_value(ROLL, -1.0, 1.0);
-			manual.pitch = get_rc_value(PITCH, -1.0, 1.0);
-			manual.yaw = get_rc_value(YAW, -1.0, 1.0);
-			manual.throttle = get_rc_value(THROTTLE, 0.0, 1.0);
+			manual.y = get_rc_value(ROLL, -1.0, 1.0);
+			manual.x = get_rc_value(PITCH, -1.0, 1.0);
+			manual.r = get_rc_value(YAW, -1.0, 1.0);
+			manual.z = get_rc_value(THROTTLE, 0.0, 1.0);
 			manual.flaps = get_rc_value(FLAPS, -1.0, 1.0);
 			manual.aux1 = get_rc_value(AUX_1, -1.0, 1.0);
 			manual.aux2 = get_rc_value(AUX_2, -1.0, 1.0);
@@ -1414,10 +1519,11 @@ Sensors::rc_poll()
 			manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0);

 			/* mode switches */
-			manual.mode_switch = get_rc_switch_position(MODE);
-			manual.assisted_switch = get_rc_switch_position(ASSISTED);
-			manual.loiter_switch = get_rc_switch_position(LOITER);
-			manual.return_switch = get_rc_switch_position(RETURN);
+			manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assist_th, _parameters.rc_assist_inv);
+			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) {
@@ -1433,10 +1539,10 @@ Sensors::rc_poll()

 			actuator_group_3.timestamp = rc_input.timestamp_last_signal;

-			actuator_group_3.control[0] = manual.roll;
-			actuator_group_3.control[1] = manual.pitch;
-			actuator_group_3.control[2] = manual.yaw;
-			actuator_group_3.control[3] = manual.throttle;
+			actuator_group_3.control[0] = manual.y;
+			actuator_group_3.control[1] = manual.x;
+			actuator_group_3.control[2] = manual.r;
+			actuator_group_3.control[3] = manual.z;
 			actuator_group_3.control[4] = manual.flaps;
 			actuator_group_3.control[5] = manual.aux1;
 			actuator_group_3.control[6] = manual.aux2;
@@ -1560,8 +1666,9 @@ Sensors::task_main()
 		diff_pres_poll(raw);

 		/* Inform other processes that new data is available to copy */
-		if (_publishing)
+		if (_publishing) {
 			orb_publish(ORB_ID(sensor_combined), _sensor_pub, &raw);
+		}

 		/* Look for new r/c input data */
 		rc_poll();
@@ -1584,7 +1691,7 @@ Sensors::start()
 	_sensors_task = task_spawn_cmd("sensors_task",
 				       SCHED_DEFAULT,
 				       SCHED_PRIORITY_MAX - 5,
-				       2048,
+				       2000,
 				       (main_t)&Sensors::task_main_trampoline,
 				       nullptr);

@@ -1598,18 +1705,21 @@ Sensors::start()

 int sensors_main(int argc, char *argv[])
 {
-	if (argc < 1)
+	if (argc < 1) {
 		errx(1, "usage: sensors {start|stop|status}");
+	}

 	if (!strcmp(argv[1], "start")) {

-		if (sensors::g_sensors != nullptr)
+		if (sensors::g_sensors != nullptr) {
 			errx(0, "already running");
+		}

 		sensors::g_sensors = new Sensors;

-		if (sensors::g_sensors == nullptr)
+		if (sensors::g_sensors == nullptr) {
 			errx(1, "alloc failed");
+		}

 		if (OK != sensors::g_sensors->start()) {
 			delete sensors::g_sensors;
@@ -1621,8 +1731,9 @@ int sensors_main(int argc, char *argv[])
 	}

 	if (!strcmp(argv[1], "stop")) {
-		if (sensors::g_sensors == nullptr)
+		if (sensors::g_sensors == nullptr) {
 			errx(1, "not running");
+		}

 		delete sensors::g_sensors;
 		sensors::g_sensors = nullptr;
@@ -1641,4 +1752,3 @@ int sensors_main(int argc, char *argv[])
 	warnx("unrecognized command");
 	return 1;
 }
-
@@ -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;
@@ -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;
-
-	/* 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 */
-
-	} 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 */
-
-	} else {
-		output_factor = max_thrust;
-	}
-
-	roll *= output_factor;
-	pitch *= output_factor;
-	yaw *= output_factor;
-
-
-	/* perform initial mix pass yielding un-bounded outputs */
+	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
 	for (unsigned i = 0; i < _rotor_count; i++) {
-		float tmp = roll  * _rotors[i].roll_scale +
+		float out = roll * _rotors[i].roll_scale +
 			    pitch * _rotors[i].pitch_scale +
-			    yaw   * _rotors[i].yaw_scale +
 			    thrust;

-		if (tmp > max)
-			max = tmp;
+		/* limit yaw if it causes outputs clipping */
+		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
+			yaw = -out / _rotors[i].yaw_scale;
+		}

-		outputs[i] = tmp;
+		/* calculate min and max output values */
+		if (out < min_out) {
+			min_out = out;
+		}
+		if (out > max_out) {
+			max_out = out;
+		}
+
+		outputs[i] = out;
 	}

-	/* scale values into the -1.0 - 1.0 range */
-	if (max > 1.0f) {
-		fixup_scale = 2.0f / max;
+	/* 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);
+
+		/* 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;
+		}

 	} else {
-		fixup_scale = 2.0f;
+		/* roll/pitch mixed without limiting, add yaw control */
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs[i] += yaw * _rotors[i].yaw_scale;
+		}
 	}

-	for (unsigned i = 0; i < _rotor_count; i++)
-		outputs[i] = -1.0f + (outputs[i] * fixup_scale);
+	/* 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;

-	/* ensure outputs are out of the deadband */
-	for (unsigned i = 0; i < _rotor_count; i++)
-		if (outputs[i] < _deadband)
-			outputs[i] = _deadband;
+	} else {
+		scale_out = 1.0f;
+	}
+
+	/* 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;
 }
@@ -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]
@@ -521,73 +521,15 @@ param_save_default(void)
 		return ERROR;
 	}

-	if (res == OK) {
-		res = param_export(fd, false);
+	res = param_export(fd, false);

-	        if (res != OK) {
-	            warnx("failed to write parameters to file: %s", filename);
-	        }
+	if (res != OK) {
+		warnx("failed to write parameters to file: %s", filename);
 	}

 	close(fd);

 	return res;
-
-#if 0
-	const char *filename_tmp = malloc(strlen(filename) + 5);
-	sprintf(filename_tmp, "%s.tmp", filename);
-
-	/* delete temp file if exist */
-	res = unlink(filename_tmp);
-
-	if (res != OK && errno == ENOENT)
-		res = OK;
-
-	if (res != OK)
-		warn("failed to delete temp file: %s", filename_tmp);
-
-	if (res == OK) {
-		/* write parameters to temp file */
-		fd = open(filename_tmp, O_WRONLY | O_CREAT | O_EXCL);
-
-		if (fd < 0) {
-			warn("failed to open temp file: %s", filename_tmp);
-			res = ERROR;
-		}
-
-		if (res == OK) {
-			res = param_export(fd, false);
-
-			if (res != OK)
-				warnx("failed to write parameters to file: %s", filename_tmp);
-		}
-
-		close(fd);
-	}
-
-	if (res == OK) {
-		/* delete parameters file */
-		res = unlink(filename);
-
-		if (res != OK && errno == ENOENT)
-			res = OK;
-
-		if (res != OK)
-			warn("failed to delete parameters file: %s", filename);
-	}
-
-	if (res == OK) {
-		/* rename temp file to parameters */
-		res = rename(filename_tmp, filename);
-
-		if (res != OK)
-			warn("failed to rename %s to %s", filename_tmp, filename);
-	}
-
-	free(filename_tmp);
-
-	return res;
-#endif
 }

 /**
@@ -281,13 +281,19 @@ 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);
 	}
 }
@@ -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 all of the performance counters.
+ * 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_all(void);
+__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(int fd);

 /**
 * Reset all of the performance counters.
@@ -53,7 +53,7 @@

 #define SIGMA 0.000001f

-__EXPORT void pid_init(PID_t *pid, uint8_t mode, float dt_min)
+__EXPORT void pid_init(PID_t *pid, pid_mode_t mode, float dt_min)
 {
 	pid->mode = mode;
 	pid->dt_min = dt_min;
@@ -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:
@@ -140,7 +140,7 @@ int rc_calibration_check(int mavlink_fd) {
 		/* 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));
+			warnx("ERROR: %d config error(s) for RC channel %d.", count, (i + 1));
 			usleep(100000);
 		}

@@ -64,22 +64,40 @@ struct manual_control_setpoint_s {
 	/**
 	 * Any of the channels may not be available and be set to NaN
 	 * to indicate that it does not contain valid data.
+	 * The variable names follow the definition of the
+	 * MANUAL_CONTROL mavlink message.
+	 * The default range is from -1 to 1 (mavlink message -1000 to 1000)
+	 * The range for the z variable is defined from 0 to 1. (The z field of
+	 * the MANUAL_CONTROL mavlink message is defined from -1000 to 1000)
 	 */
-	float roll;			 	/**< ailerons roll / roll rate input, -1..1 */
-	float pitch;			/**< elevator / pitch / pitch rate, -1..1 */
-	float yaw;				/**< rudder / yaw rate / yaw, -1..1 */
-	float throttle;			/**< throttle / collective thrust / altitude, 0..1 */
+	float x;			/**< stick position in x direction -1..1
+					  in general corresponds to forward/back motion or pitch of vehicle,
+					  in general a positive value means forward or negative pitch and
+					  a negative value means backward or positive pitch */
+	float y;			/**< stick position in y direction -1..1
+					  in general corresponds to right/left motion or roll of vehicle,
+					  in general a positive value means right or positive roll and
+					  a negative value means left or negative roll */
+	float z;			/**< throttle stick position 0..1
+					  in general corresponds to up/down motion or thrust of vehicle,
+					  in general the value corresponds to the demanded throttle by the user,
+					  if the input is used for setting the setpoint of a vertical position
+					  controller any value > 0.5 means up and any value < 0.5 means down */
+	float r;			/**< yaw stick/twist positon, -1..1
+					  in general corresponds to the righthand rotation around the vertical
+					  (downwards) axis of the vehicle */
 	float flaps;			/**< flap position */
-	float aux1;				/**< default function: camera yaw / azimuth */
-	float aux2;				/**< default function: camera pitch / tilt */
-	float aux3;				/**< default function: camera trigger */
-	float aux4;				/**< default function: camera roll */
-	float aux5;				/**< default function: payload drop */
+	float aux1;			/**< default function: camera yaw / azimuth */
+	float aux2;			/**< default function: camera pitch / tilt */
+	float aux3;			/**< default function: camera trigger */
+	float aux4;			/**< default function: camera roll */
+	float aux5;			/**< default function: payload drop */

-	switch_pos_t mode_switch;			/**< mode 3 position switch (mandatory): manual, assisted, auto */
-	switch_pos_t return_switch;			/**< land 2 position switch (mandatory): land, no effect */
-	switch_pos_t assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
-	switch_pos_t loiter_switch;		/**< mission 2 position switch (optional): mission, loiter */
+	switch_pos_t mode_switch;			/**< main mode 3 position switch (mandatory): _MANUAL_, ASSIST, AUTO */
+	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 */

 /**
@@ -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.
@@ -64,15 +64,16 @@ enum RC_CHANNELS_FUNCTION {
 	YAW      = 3,
 	MODE = 4,
 	RETURN = 5,
-	ASSISTED = 6,
+	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. */
 };

@@ -68,6 +68,9 @@ struct vehicle_gps_position_s {
 	float eph;					/**< GPS HDOP horizontal dilution of position in m */
 	float epv;					/**< 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 */
 };

@@ -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;
 };

 /**
@@ -61,9 +61,10 @@
 /* main state machine */
 typedef enum {
 	MAIN_STATE_MANUAL = 0,
-	MAIN_STATE_SEATBELT,
-	MAIN_STATE_EASY,
+	MAIN_STATE_ALTCTL,
+	MAIN_STATE_POSCTL,
 	MAIN_STATE_AUTO,
+	MAIN_STATE_ACRO,
 	MAIN_STATE_MAX
 } main_state_t;