From 7f4f9a5f5f6c4921453f3e0c98531bcb4e44d981 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 31 Jan 2014 22:44:05 +0100
Subject: [PATCH 01/34] ACRO mode implemented

---
 src/modules/commander/commander.cpp           |  35 +++++-
 .../commander/state_machine_helper.cpp        |   4 +
 .../mc_att_control/mc_att_control_main.cpp    | 102 +++++++++++++-----
 .../mc_att_control/mc_att_control_params.c    |   3 +
 src/modules/sensors/sensor_params.c           |   1 +
 src/modules/sensors/sensors.cpp               |  14 +++
 .../uORB/topics/manual_control_setpoint.h     |   1 +
 src/modules/uORB/topics/rc_channels.h         |  11 +-
 src/modules/uORB/topics/vehicle_status.h      |   8 ++
 9 files changed, 144 insertions(+), 35 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index c39833713d..89294fc76c 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -619,9 +619,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[3] = "AUTO";
+	main_states_str[1] = "ACRO";
+	main_states_str[2] = "SEATBELT";
+	main_states_str[3] = "EASY";
+	main_states_str[4] = "AUTO";
 
 	char *arming_states_str[ARMING_STATE_MAX];
 	arming_states_str[0] = "INIT";
@@ -1510,6 +1511,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta
 	} else {
 		status->mission_switch = MISSION_SWITCH_MISSION;
 	}
+
+	/* acro switch  */
+	if (!isfinite(sp_man->acro_switch)) {
+		status->acro_switch = ACRO_SWITCH_NONE;
+
+	} else if (sp_man->acro_switch > STICK_ON_OFF_LIMIT) {
+		status->acro_switch = ACRO_SWITCH_ACRO;
+
+	} else {
+		status->acro_switch = ACRO_SWITCH_NORMAL;
+	}
 }
 
 transition_result_t
@@ -1520,7 +1532,11 @@ set_main_state_rc(struct vehicle_status_s *status)
 
 	switch (status->mode_switch) {
 	case MODE_SWITCH_MANUAL:
-		res = main_state_transition(status, MAIN_STATE_MANUAL);
+		if (status->acro_switch == ACRO_SWITCH_ACRO) {
+			res = main_state_transition(status, MAIN_STATE_ACRO);
+		} else {
+			res = main_state_transition(status, MAIN_STATE_MANUAL);
+		}
 		// TRANSITION_DENIED is not possible here
 		break;
 
@@ -1600,6 +1616,17 @@ set_control_mode()
 			control_mode.flag_control_velocity_enabled = false;
 			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;
+
 		case MAIN_STATE_SEATBELT:
 			control_mode.flag_control_manual_enabled = true;
 			control_mode.flag_control_auto_enabled = false;
diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 43d0e023e3..fd966a0682 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -230,6 +230,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 		ret = TRANSITION_CHANGED;
 		break;
 
+	case MAIN_STATE_ACRO:
+		ret = TRANSITION_CHANGED;
+		break;
+
 	case MAIN_STATE_SEATBELT:
 
 		/* need at minimum altitude estimate */
diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index a0accb8552..5f862652a3 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -161,7 +161,12 @@ private:
 		param_t yaw_rate_i;
 		param_t yaw_rate_d;
 		param_t yaw_ff;
+		param_t roll_scale_acro;
+		param_t pitch_scale_acro;
+		param_t yaw_scale_acro;
 
+		param_t rc_scale_roll;
+		param_t rc_scale_pitch;
 		param_t rc_scale_yaw;
 	}		_params_handles;		/**< handles for interesting parameters */
 
@@ -171,8 +176,9 @@ 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 */
+		math::Vector<3> scale_acro;			/**< scale for ACRO mode */
 
-		float rc_scale_yaw;
+		math::Vector<3> rc_scale;			/**< scale for MANUAL mode */
 	}		_params;
 
 	/**
@@ -275,6 +281,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.rate_p.zero();
 	_params.rate_i.zero();
 	_params.rate_d.zero();
+	_params.scale_acro.zero();
+	_params.rc_scale.zero();
 
 	_R_sp.identity();
 	_R.identity();
@@ -286,21 +294,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 
 	I.identity();
 
-	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
-	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
-	_params_handles.roll_rate_i		= 	param_find("MC_ROLLRATE_I");
-	_params_handles.roll_rate_d		= 	param_find("MC_ROLLRATE_D");
-	_params_handles.pitch_p			= 	param_find("MC_PITCH_P");
-	_params_handles.pitch_rate_p	= 	param_find("MC_PITCHRATE_P");
-	_params_handles.pitch_rate_i	= 	param_find("MC_PITCHRATE_I");
-	_params_handles.pitch_rate_d	= 	param_find("MC_PITCHRATE_D");
-	_params_handles.yaw_p			=	param_find("MC_YAW_P");
-	_params_handles.yaw_rate_p		= 	param_find("MC_YAWRATE_P");
-	_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.roll_p				= 	param_find("MC_ROLL_P");
+	_params_handles.roll_rate_p			= 	param_find("MC_ROLLRATE_P");
+	_params_handles.roll_rate_i			= 	param_find("MC_ROLLRATE_I");
+	_params_handles.roll_rate_d			= 	param_find("MC_ROLLRATE_D");
+	_params_handles.pitch_p				= 	param_find("MC_PITCH_P");
+	_params_handles.pitch_rate_p		= 	param_find("MC_PITCHRATE_P");
+	_params_handles.pitch_rate_i		= 	param_find("MC_PITCHRATE_I");
+	_params_handles.pitch_rate_d		= 	param_find("MC_PITCHRATE_D");
+	_params_handles.yaw_p				=	param_find("MC_YAW_P");
+	_params_handles.yaw_rate_p			= 	param_find("MC_YAWRATE_P");
+	_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.roll_scale_acro		= 	param_find("MC_ROLL_S_ACRO");
+	_params_handles.pitch_scale_acro	= 	param_find("MC_PITCH_S_ACRO");
+	_params_handles.yaw_scale_acro		= 	param_find("MC_YAW_S_ACRO");
 
-	_params_handles.rc_scale_yaw	= 	param_find("RC_SCALE_YAW");
+	_params_handles.rc_scale_roll		= 	param_find("RC_SCALE_ROLL");
+	_params_handles.rc_scale_pitch		= 	param_find("RC_SCALE_PITCH");
+	_params_handles.rc_scale_yaw		= 	param_find("RC_SCALE_YAW");
 
 	/* fetch initial parameter values */
 	parameters_update();
@@ -344,6 +357,10 @@ MulticopterAttitudeControl::parameters_update()
 	_params.rate_i(0) = v;
 	param_get(_params_handles.roll_rate_d, &v);
 	_params.rate_d(0) = v;
+	param_get(_params_handles.roll_scale_acro, &v);
+	_params.scale_acro(0) = v;
+	param_get(_params_handles.rc_scale_roll, &v);
+	_params.rc_scale(0) = v;
 
 	/* pitch */
 	param_get(_params_handles.pitch_p, &v);
@@ -354,6 +371,10 @@ MulticopterAttitudeControl::parameters_update()
 	_params.rate_i(1) = v;
 	param_get(_params_handles.pitch_rate_d, &v);
 	_params.rate_d(1) = v;
+	param_get(_params_handles.pitch_scale_acro, &v);
+	_params.scale_acro(1) = v;
+	param_get(_params_handles.rc_scale_pitch, &v);
+	_params.rc_scale(1) = v;
 
 	/* yaw */
 	param_get(_params_handles.yaw_p, &v);
@@ -364,10 +385,11 @@ MulticopterAttitudeControl::parameters_update()
 	_params.rate_i(2) = v;
 	param_get(_params_handles.yaw_rate_d, &v);
 	_params.rate_d(2) = v;
-
 	param_get(_params_handles.yaw_ff, &_params.yaw_ff);
-
-	param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw);
+	param_get(_params_handles.yaw_scale_acro, &v);
+	_params.scale_acro(2) = v;
+	param_get(_params_handles.rc_scale_yaw, &v);
+	_params.rc_scale(2) = v;
 
 	return OK;
 }
@@ -463,6 +485,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
 
 	if (_v_control_mode.flag_control_manual_enabled) {
 		/* manual input, set or modify attitude setpoint */
+		vehicle_manual_poll();
 
 		if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
@@ -488,16 +511,16 @@ MulticopterAttitudeControl::control_attitude(float dt)
 			//	reset_yaw_sp = true;
 			//}
 		} else {
-			float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw;
-			if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
+			float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale(2);
+			if (_params.rc_scale(2) > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) {
 				/* move yaw setpoint */
-				yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw;
+				yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale(2);
 				if (_manual_control_sp.yaw > 0.0f) {
 					yaw_sp_move_rate -= YAW_DEADZONE;
 				} else {
 					yaw_sp_move_rate += YAW_DEADZONE;
 				}
-				yaw_sp_move_rate *= _params.rc_scale_yaw;
+				yaw_sp_move_rate *= _params.rc_scale(2);
 				_v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 				_v_att_sp.R_valid = false;
 				publish_att_sp = true;
@@ -743,7 +766,6 @@ MulticopterAttitudeControl::task_main()
 			parameter_update_poll();
 			vehicle_control_mode_poll();
 			arming_status_poll();
-			vehicle_manual_poll();
 
 			if (_v_control_mode.flag_control_attitude_enabled) {
 				control_attitude(dt);
@@ -764,9 +786,37 @@ MulticopterAttitudeControl::task_main()
 
 			} else {
 				/* attitude controller disabled */
-				// TODO poll 'attitude_rates_setpoint' topic
-				_rates_sp.zero();
-				_thrust_sp = 0.0f;
+				if (_v_control_mode.flag_control_manual_enabled) {
+					/* manual rates control, ACRO mode */
+					vehicle_manual_poll();
+
+					_rates_sp(0) = _manual_control_sp.roll;
+					_rates_sp(1) = _manual_control_sp.pitch;
+					_rates_sp(2) = _manual_control_sp.yaw;
+
+					/* rescale controls for ACRO mode */
+					_rates_sp = _rates_sp.edivide(_params.rc_scale).emult(_params.scale_acro);
+					_thrust_sp = _manual_control_sp.throttle;
+
+					/* 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 {
+					// TODO poll 'attitude_rates_setpoint' topic
+					_rates_sp.zero();
+					_thrust_sp = 0.0f;
+				}
 			}
 
 			if (_v_control_mode.flag_control_rates_enabled) {
diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c
index 27a45b6bbc..f3a4022c84 100644
--- a/src/modules/mc_att_control/mc_att_control_params.c
+++ b/src/modules/mc_att_control/mc_att_control_params.c
@@ -54,3 +54,6 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f);
 PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f);
 PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f);
+PARAM_DEFINE_FLOAT(MC_ROLL_S_ACRO, 5.0f);
+PARAM_DEFINE_FLOAT(MC_PITCH_S_ACRO, 5.0f);
+PARAM_DEFINE_FLOAT(MC_YAW_S_ACRO, 3.0f);
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 30659fd3a0..e72b48a881 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -449,6 +449,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0);
 PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0);
+PARAM_DEFINE_INT32(RC_MAP_ACRO_SW, 0);
 
 //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0);
 
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index bd665b592b..50ddec8a94 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -246,6 +246,7 @@ private:
 		int rc_map_return_sw;
 		int rc_map_assisted_sw;
 		int rc_map_mission_sw;
+		int rc_map_acro_sw;
 
 //		int rc_map_offboard_ctrl_mode_sw;
 
@@ -296,6 +297,7 @@ private:
 		param_t rc_map_return_sw;
 		param_t rc_map_assisted_sw;
 		param_t rc_map_mission_sw;
+		param_t rc_map_acro_sw;
 
 //		param_t rc_map_offboard_ctrl_mode_sw;
 
@@ -515,6 +517,7 @@ Sensors::Sensors() :
 	/* optional mode switches, not mapped per default */
 	_parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW");
 	_parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW");
+	_parameter_handles.rc_map_acro_sw = param_find("RC_MAP_ACRO_SW");
 
 //	_parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW");
 
@@ -669,6 +672,10 @@ Sensors::parameters_update()
 		warnx("Failed getting mission sw chan index");
 	}
 
+	if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) {
+		warnx("Failed getting acro sw chan index");
+	}
+
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
 		warnx("Failed getting flaps chan index");
 	}
@@ -700,6 +707,7 @@ Sensors::parameters_update()
 	_rc.function[RETURN] = _parameters.rc_map_return_sw - 1;
 	_rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1;
 	_rc.function[MISSION] = _parameters.rc_map_mission_sw - 1;
+	_rc.function[ACRO] = _parameters.rc_map_acro_sw - 1;
 
 	_rc.function[FLAPS] = _parameters.rc_map_flaps - 1;
 
@@ -1290,6 +1298,7 @@ Sensors::rc_poll()
 		manual_control.return_switch = NAN;
 		manual_control.assisted_switch = NAN;
 		manual_control.mission_switch = NAN;
+		manual_control.acro_switch = NAN;
 //		manual_control.auto_offboard_input_switch = NAN;
 
 		manual_control.flaps = NAN;
@@ -1428,6 +1437,11 @@ Sensors::rc_poll()
 			manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled);
 		}
 
+		/* acro switch input */
+		if (_rc.function[ACRO] >= 0) {
+			manual_control.acro_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ACRO]].scaled);
+		}
+
 		/* return switch input */
 		if (_rc.function[RETURN] >= 0) {
 			manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled);
diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h
index eac6d6e986..190dc01c8e 100644
--- a/src/modules/uORB/topics/manual_control_setpoint.h
+++ b/src/modules/uORB/topics/manual_control_setpoint.h
@@ -60,6 +60,7 @@ struct manual_control_setpoint_s {
 	float return_switch;			/**< land 2 position switch (mandatory): land, no effect */
 	float assisted_switch;			/**< assisted 2 position switch (optional): seatbelt, simple */
 	float mission_switch;		/**< mission 2 position switch (optional): mission, loiter */
+	float acro_switch;			/**< acro 2 position switch (optional): normal, acro */
 
 	/**
 	 * Any of the channels below may not be available and be set to NaN
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index 086b2ef150..beb7008ab9 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -68,11 +68,12 @@ enum RC_CHANNELS_FUNCTION
   ASSISTED = 6,
   MISSION = 7,
   OFFBOARD_MODE = 8,
-  FLAPS   = 9,
-  AUX_1   = 10,
-  AUX_2   = 11,
-  AUX_3   = 12,
-  AUX_4   = 13,
+  ACRO	= 9,
+  FLAPS   = 10,
+  AUX_1   = 11,
+  AUX_2   = 12,
+  AUX_3   = 13,
+  AUX_4   = 14,
   AUX_5   = 14,
   RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h
index 1b3639e309..5cb0bd8a20 100644
--- a/src/modules/uORB/topics/vehicle_status.h
+++ b/src/modules/uORB/topics/vehicle_status.h
@@ -63,6 +63,7 @@
 /* main state machine */
 typedef enum {
 	MAIN_STATE_MANUAL = 0,
+	MAIN_STATE_ACRO,
 	MAIN_STATE_SEATBELT,
 	MAIN_STATE_EASY,
 	MAIN_STATE_AUTO,
@@ -116,6 +117,12 @@ typedef enum {
 	MISSION_SWITCH_MISSION
 } mission_switch_pos_t;
 
+typedef enum {
+	ACRO_SWITCH_NONE = 0,
+	ACRO_SWITCH_NORMAL,
+	ACRO_SWITCH_ACRO
+} acro_switch_pos_t;
+
 enum VEHICLE_MODE_FLAG {
 	VEHICLE_MODE_FLAG_SAFETY_ARMED = 128,
 	VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64,
@@ -192,6 +199,7 @@ struct vehicle_status_s
 	return_switch_pos_t return_switch;
 	assisted_switch_pos_t assisted_switch;
 	mission_switch_pos_t mission_switch;
+	acro_switch_pos_t acro_switch;
 
 	bool condition_battery_voltage_valid;
 	bool condition_system_in_air_restore;	/**< true if we can restore in mid air */

From 2923bdf39fd6e424523f0b6b47bef3cabcdc0645 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sat, 1 Feb 2014 11:25:29 +0100
Subject: [PATCH 02/34] commander: allow disarming in ACRO without landing (as
 in MANUAL)

---
 src/modules/commander/commander.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 89294fc76c..30f75b9fb1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1120,7 +1120,7 @@ int commander_thread_main(int argc, char *argv[])
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
 			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-			    (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
+			    (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
 			    sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) {
 
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {

From 21300874ac6d337d85c49704e9233fdd17192171 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sun, 2 Feb 2014 23:07:48 +0100
Subject: [PATCH 03/34] mc_att_control: reset yaw setpoint after ACRO

---
 src/modules/mc_att_control/mc_att_control_main.cpp | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 21e1016625..77531cbb92 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -802,6 +802,8 @@ MulticopterAttitudeControl::task_main()
 					_rates_sp = _rates_sp.edivide(_params.rc_scale).emult(_params.scale_acro);
 					_thrust_sp = _manual_control_sp.throttle;
 
+					_reset_yaw_sp = true;
+
 					/* publish attitude rates setpoint */
 					_v_rates_sp.roll = _rates_sp(0);
 					_v_rates_sp.pitch = _rates_sp(1);

From 6e7136c6b3f953c26cf75c9f8b777a6a7c84ea9a Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 4 Feb 2014 21:50:26 +0100
Subject: [PATCH 04/34] rc_channels topic: bug fixed; sensors: minor cleanup

---
 src/modules/sensors/sensors.cpp       |  2 +-
 src/modules/uORB/topics/rc_channels.h | 34 +++++++++++++--------------
 2 files changed, 18 insertions(+), 18 deletions(-)

diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index b37a744ca2..73f14225de 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -674,7 +674,7 @@ Sensors::parameters_update()
 	}
 
 	if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) {
-		warnx("Failed getting acro sw chan index");
+		warnx(paramerr);
 	}
 
 	if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) {
diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h
index beb7008ab9..36106751e2 100644
--- a/src/modules/uORB/topics/rc_channels.h
+++ b/src/modules/uORB/topics/rc_channels.h
@@ -45,12 +45,12 @@
 /**
  * The number of RC channel inputs supported.
  * Current (Q4/2013) radios support up to 18 channels,
- * leaving at a sane value of 15.
+ * leaving at a sane value of 16.
  * This number can be greater then number of RC channels,
  * because single RC channel can be mapped to multiple
  * functions, e.g. for various mode switches.
  */
-#define RC_CHANNELS_MAPPED_MAX   15
+#define RC_CHANNELS_MAPPED_MAX   16
 
 /** 
  * This defines the mapping of the RC functions.
@@ -60,21 +60,21 @@
 enum RC_CHANNELS_FUNCTION
 {
   THROTTLE = 0,
-  ROLL     = 1,
-  PITCH    = 2,
-  YAW      = 3,
-  MODE = 4,
-  RETURN = 5,
-  ASSISTED = 6,
-  MISSION = 7,
-  OFFBOARD_MODE = 8,
-  ACRO	= 9,
-  FLAPS   = 10,
-  AUX_1   = 11,
-  AUX_2   = 12,
-  AUX_3   = 13,
-  AUX_4   = 14,
-  AUX_5   = 14,
+  ROLL,
+  PITCH,
+  YAW,
+  MODE,
+  RETURN,
+  ASSISTED,
+  MISSION,
+  OFFBOARD_MODE,
+  ACRO,
+  FLAPS,
+  AUX_1,
+  AUX_2,
+  AUX_3,
+  AUX_4,
+  AUX_5,
   RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */
 };
 

From 3b2b270a40e0d8528339fe7cde5e0af91684fb97 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 4 Feb 2014 21:55:12 +0100
Subject: [PATCH 05/34] mavlink: custom mode ACRO added

---
 src/modules/commander/px4_custom_mode.h | 1 +
 src/modules/mavlink/mavlink.c           | 3 +++
 2 files changed, 4 insertions(+)

diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h
index b60a7e0b9c..d69ab30677 100644
--- a/src/modules/commander/px4_custom_mode.h
+++ b/src/modules/commander/px4_custom_mode.h
@@ -13,6 +13,7 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_SEATBELT,
 	PX4_CUSTOM_MAIN_MODE_EASY,
 	PX4_CUSTOM_MAIN_MODE_AUTO,
+	PX4_CUSTOM_MAIN_MODE_ACRO,
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {
diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c
index 4d975066f7..22465a3dae 100644
--- a/src/modules/mavlink/mavlink.c
+++ b/src/modules/mavlink/mavlink.c
@@ -212,6 +212,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u
 		if (v_status.main_state == MAIN_STATE_MANUAL) {
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0);
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL;
+		} else if (v_status.main_state == MAIN_STATE_ACRO) {
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
 		} else if (v_status.main_state == MAIN_STATE_SEATBELT) {
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED;
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT;

From b60964eb9c1e24b14c0cbf4527bf6b7e4bb5fd40 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 14 May 2014 13:27:53 +0200
Subject: [PATCH 06/34] Multirotor mixer: more careful limiting

---
 .../systemlib/mixer/mixer_multirotor.cpp      | 70 +++++++++----------
 1 file changed, 33 insertions(+), 37 deletions(-)

diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index bf77795d5c..740a22781e 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -282,56 +282,52 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	float		yaw     = get_control(0, 2) * _yaw_scale;
 	float		thrust  = get_control(0, 3);
 	//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
-	float		max     = 0.0f;
+	float		min_out = 0.0f;
+	float		max_out = 0.0f;
 	float		fixup_scale;
 
-	/* 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 */
 	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 +
+			    yaw * _rotors[i].yaw_scale +
 			    thrust;
 
-		if (tmp > max)
-			max = tmp;
+		if (out < min_out) {
+			min_out = out;
+		}
+		if (out > max_out) {
+			max_out = out;
+		}
 
-		outputs[i] = tmp;
+		outputs[i] = out;
 	}
 
-	/* scale values into the -1.0 - 1.0 range */
-	if (max > 1.0f) {
-		fixup_scale = 2.0f / max;
+	/* scale down controls if some outputs are negative, 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 +
+				    yaw * _rotors[i].yaw_scale) +
+				    thrust;
+		}
+	}
+
+	/* scale down all outputs if some outputs are too large, reduce total thrust */
+	float scale_out;
+	if (max_out > 1.0f) {
+		scale_out = 1.0f / max_out;
 
 	} else {
-		fixup_scale = 2.0f;
+		scale_out = 1.0f;
 	}
 
-	for (unsigned i = 0; i < _rotor_count; i++)
-		outputs[i] = -1.0f + (outputs[i] * fixup_scale);
+	for (unsigned i = 0; i < _rotor_count; i++) {
+		outputs[i] = -1.0f + (outputs[i] * 2 * scale_out);
+	}
 
 	/* ensure outputs are out of the deadband */
 	for (unsigned i = 0; i < _rotor_count; i++)

From ae1faa6de6d6952af73a8a9367625fbf96822fe1 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Wed, 14 May 2014 13:45:43 +0200
Subject: [PATCH 07/34] MC mixer input limiting implemented.

---
 src/modules/systemlib/mixer/mixer_multirotor.cpp | 13 +++++++++----
 1 file changed, 9 insertions(+), 4 deletions(-)

diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 740a22781e..2af9d913d3 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -67,6 +67,11 @@
 namespace
 {
 
+float constrain(float val, float min, float max)
+{
+	return (val < min) ? min : ((val > max) ? max : val);
+}
+
 /*
  * These tables automatically generated by multi_tables - do not edit.
  */
@@ -276,11 +281,11 @@ 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		min_out = 0.0f;
 	float		max_out = 0.0f;

From b9b84b08b79dd6661905cfd5d4fa8578ea392bec Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 15 May 2014 14:01:55 +0200
Subject: [PATCH 08/34] Multirotor mixer: limit yaw first, then roll/pitch

---
 .../systemlib/mixer/mixer_multirotor.cpp      | 24 ++++++++++++-------
 1 file changed, 16 insertions(+), 8 deletions(-)

diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 2af9d913d3..672784f468 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -289,15 +289,20 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 	//lowsyslog("thrust: %d, get_control3: %d\n", (int)(thrust), (int)(get_control(0, 3)));
 	float		min_out = 0.0f;
 	float		max_out = 0.0f;
-	float		fixup_scale;
+	float		scale_yaw = 1.0f;
 
-	/* perform initial mix pass yielding unbounded outputs */
+	/* perform initial mix pass yielding unbounded outputs, ignore yaw */
 	for (unsigned i = 0; i < _rotor_count; i++) {
 		float out = roll * _rotors[i].roll_scale +
 			    pitch * _rotors[i].pitch_scale +
-			    yaw * _rotors[i].yaw_scale +
 			    thrust;
 
+		/* limit yaw if it causes outputs clipping */
+		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
+			yaw = out / _rotors[i].yaw_scale;
+		}
+
+		/* calculate min and max output values */
 		if (out < min_out) {
 			min_out = out;
 		}
@@ -308,16 +313,19 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		outputs[i] = out;
 	}
 
-	/* scale down controls if some outputs are negative, keep total thrust */
+	/* scale down roll/pitch controls if some outputs are negative, don't add yaw, keep total thrust */
 	if (min_out < 0.0) {
 		float scale_in = thrust / (thrust - min_out);
 
 		/* 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 +
-				    yaw * _rotors[i].yaw_scale) +
-				    thrust;
+			outputs[i] = scale_in * (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) + thrust;
+		}
+
+	} else {
+		/* roll/pitch mixed without limiting, add yaw control */
+		for (unsigned i = 0; i < _rotor_count; i++) {
+			outputs[i] += yaw * _rotors[i].yaw_scale;
 		}
 	}
 

From bc3ca8db5646cf2a2e235cf7ca3bd62335e062c2 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 15 May 2014 14:26:32 +0200
Subject: [PATCH 09/34] Multirotor mixer: yaw limiting bug fixed

---
 src/modules/systemlib/mixer/mixer_multirotor.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 672784f468..8568f9e60f 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -299,7 +299,7 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 
 		/* limit yaw if it causes outputs clipping */
 		if (out >= 0.0f && out < -yaw * _rotors[i].yaw_scale) {
-			yaw = out / _rotors[i].yaw_scale;
+			yaw = -out / _rotors[i].yaw_scale;
 		}
 
 		/* calculate min and max output values */

From d9a7e528b056556112c74d13a86f30bdab88f635 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 15 May 2014 15:44:56 +0200
Subject: [PATCH 10/34] Multirotor mixer: idle_speed (aka deadband) fixed

---
 src/modules/systemlib/mixer/mixer.h              |  2 +-
 src/modules/systemlib/mixer/mixer_multirotor.cpp | 12 ++++--------
 2 files changed, 5 insertions(+), 9 deletions(-)

diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 1c889a8119..fa9d28b74c 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -516,7 +516,7 @@ private:
 	float				_roll_scale;
 	float				_pitch_scale;
 	float				_yaw_scale;
-	float				_deadband;
+	float				_idle_speed;
 
 	unsigned			_rotor_count;
 	const Rotor			*_rotors;
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 8568f9e60f..1ca0a21e97 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -176,12 +176,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])
 {
@@ -338,15 +338,11 @@ MultirotorMixer::mix(float *outputs, unsigned space)
 		scale_out = 1.0f;
 	}
 
+	/* scale outputs to range _idle_speed..1 */
 	for (unsigned i = 0; i < _rotor_count; i++) {
-		outputs[i] = -1.0f + (outputs[i] * 2 * scale_out);
+		outputs[i] = _idle_speed + (outputs[i] * (1.0f - _idle_speed) * scale_out);
 	}
 
-	/* ensure outputs are out of the deadband */
-	for (unsigned i = 0; i < _rotor_count; i++)
-		if (outputs[i] < _deadband)
-			outputs[i] = _deadband;
-
 	return _rotor_count;
 }
 

From b903fc1ed3966400b9a585fd155bf844557fc345 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 09:50:10 +0200
Subject: [PATCH 11/34] px4io: Improve the documentation of the protocol
 header, NO FUNCTIONAL CHANGES

---
 src/modules/px4iofirmware/protocol.h | 11 ++++++-----
 1 file changed, 6 insertions(+), 5 deletions(-)

diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 91975f2a07..313084fe9d 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -209,15 +209,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 */

From 4fe0027e7a23fdd425479c98f0167adf596d5743 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 09:51:23 +0200
Subject: [PATCH 12/34] px4io driver: Only try to upload if we have a non-zero
 failsafe throttle value

---
 src/drivers/px4io/px4io.cpp | 14 +++++++++-----
 1 file changed, 9 insertions(+), 5 deletions(-)

diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index da454eb853..4099e55226 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -994,13 +994,17 @@ PX4IO::task_main()
 				int32_t failsafe_param_val;
 				param_t failsafe_param = param_find("RC_FAILS_THR");
 
-				if (failsafe_param > 0) {
+				if (failsafe_param != PARAM_INVALID) {
 
 					param_get(failsafe_param, &failsafe_param_val);
-					uint16_t failsafe_thr = failsafe_param_val;
-					pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
-					if (pret != OK) {
-						log("failsafe upload failed");
+
+					if (failsafe_param_val > 0) {
+
+						uint16_t failsafe_thr = failsafe_param_val;
+						pret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RC_THR_FAILSAFE_US, &failsafe_thr, 1);
+						if (pret != OK) {
+							log("failsafe upload failed, FS: %d us", (int)failsafe_thr);
+						}
 					}
 				}
 

From 5ca96631fc02d3826d2ef6274f9da7e12e9fb4a1 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 09:52:46 +0200
Subject: [PATCH 13/34] px4io firmware: Actually accept failsafe values, check
 their range. Add an error return code for commands that were rejected on
 reboot and force safety instead of failing silently with an OK value

---
 src/modules/px4iofirmware/registers.c | 17 +++++++++++++++--
 1 file changed, 15 insertions(+), 2 deletions(-)

diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index fd7c6081f6..a43bb65a41 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -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 to be safe */
+	[PX4IO_P_SETUP_RELAYS]			= 0,
 #endif
 #ifdef ADC_VSERVO
 	[PX4IO_P_SETUP_VSERVO_SCALE]		= 10000,
@@ -562,12 +565,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
 			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 				// don't allow reboot while armed
-				break;
+				return -1;
 			}
 
 			// check the magic value
 			if (value != PX4IO_REBOOT_BL_MAGIC)
-				break;
+				return -1;
 
                         // we schedule a reboot rather than rebooting
                         // immediately to allow the IO board to ACK
@@ -582,6 +585,16 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
 			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
+			} else {
+				return -1;
+			}
+			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;
+			} else {
+				return -1;
 			}
 			break;
 

From 1b491005d1192bfee7cf25068195b5f5e3a604eb Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 10:43:50 +0200
Subject: [PATCH 14/34] Minor cleanups on handling, make wrong reg accesses
 non-fatal for the transfer

---
 src/modules/px4iofirmware/protocol.h  |  2 ++
 src/modules/px4iofirmware/registers.c | 27 ++++++++++++++-------------
 2 files changed, 16 insertions(+), 13 deletions(-)

diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h
index 313084fe9d..7471faec7b 100644
--- a/src/modules/px4iofirmware/protocol.h
+++ b/src/modules/px4iofirmware/protocol.h
@@ -190,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) */
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index a43bb65a41..db1836f4a5 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -161,8 +161,8 @@ volatile uint16_t	r_page_setup[] =
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V1
 	[PX4IO_P_SETUP_RELAYS]			= 0,
 #else
-	/* this is unused, but we will pad it to be safe */
-	[PX4IO_P_SETUP_RELAYS]			= 0,
+	/* 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,
@@ -526,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;
 
@@ -565,12 +569,13 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			if ((r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ||
 			    (r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) {
 				// don't allow reboot while armed
-				return -1;
+				break;
 			}
 
 			// check the magic value
-			if (value != PX4IO_REBOOT_BL_MAGIC)
-				return -1;
+			if (value != PX4IO_REBOOT_BL_MAGIC) {
+				break;
+			}
 
                         // we schedule a reboot rather than rebooting
                         // immediately to allow the IO board to ACK
@@ -585,16 +590,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 		case PX4IO_P_SETUP_FORCE_SAFETY_OFF:
 			if (value == PX4IO_FORCE_SAFETY_MAGIC) {
 				r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF;
-			} else {
-				return -1;
 			}
 			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;
-			} else {
-				return -1;
 			}
 			break;
 

From f66cd1a81400c80166aae8a65552370bf206c11b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 10:44:44 +0200
Subject: [PATCH 15/34] frsky telem: reduce stack, fix missing header

---
 src/drivers/frsky_telemetry/frsky_data.c      | 2 ++
 src/drivers/frsky_telemetry/frsky_telemetry.c | 2 +-
 src/drivers/frsky_telemetry/module.mk         | 2 ++
 3 files changed, 5 insertions(+), 1 deletion(-)

diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index 57a03bc844..18de3f4dac 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -53,6 +53,8 @@
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/vehicle_status.h>
 
+#include <drivers/drv_hrt.h>
+
 /* FrSky sensor hub data IDs */
 #define FRSKY_ID_GPS_ALT_BP     0x01
 #define FRSKY_ID_TEMP1          0x02
diff --git a/src/drivers/frsky_telemetry/frsky_telemetry.c b/src/drivers/frsky_telemetry/frsky_telemetry.c
index 7b08ca69e9..6e08390436 100644
--- a/src/drivers/frsky_telemetry/frsky_telemetry.c
+++ b/src/drivers/frsky_telemetry/frsky_telemetry.c
@@ -222,7 +222,7 @@ int frsky_telemetry_main(int argc, char *argv[])
 		frsky_task = task_spawn_cmd("frsky_telemetry",
 					  SCHED_DEFAULT,
 					  SCHED_PRIORITY_DEFAULT,
-					  2048,
+					  2000,
 					  frsky_telemetry_thread_main,
 					  (const char **)argv);
 
diff --git a/src/drivers/frsky_telemetry/module.mk b/src/drivers/frsky_telemetry/module.mk
index 1632c74f75..9a49589ee5 100644
--- a/src/drivers/frsky_telemetry/module.mk
+++ b/src/drivers/frsky_telemetry/module.mk
@@ -39,3 +39,5 @@ MODULE_COMMAND	= frsky_telemetry
 
 SRCS		= frsky_data.c \
               frsky_telemetry.c
+
+MODULE_STACKSIZE = 1200

From 842819a6d6984583237f0ebcbc3bd77354574cd7 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 10:45:00 +0200
Subject: [PATCH 16/34] Use smaller logging buffer

---
 ROMFS/px4fmu_common/init.d/rc.logging | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging
index 009e7431ac..517e073afb 100644
--- a/ROMFS/px4fmu_common/init.d/rc.logging
+++ b/ROMFS/px4fmu_common/init.d/rc.logging
@@ -8,7 +8,7 @@ then
 	if ver hwcmp PX4FMU_V1
 	then
 		echo "Start sdlog2 at 50Hz"
-		sdlog2 start -r 50 -a -b 5 -t
+		sdlog2 start -r 50 -a -b 4 -t
 	else
 		echo "Start sdlog2 at 200Hz"
 		sdlog2 start -r 200 -a -b 16 -t

From 0a6861e98ddb5f25c6564e2251bb05dc0606bfc3 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 10:45:37 +0200
Subject: [PATCH 17/34] FMUv1: Reduce user main stack slightly, still 2.5K
 buffer between actual use and size

---
 nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig
index c599b118f7..bc0c120679 100644
--- a/nuttx-configs/px4fmu-v1/nsh/defconfig
+++ b/nuttx-configs/px4fmu-v1/nsh/defconfig
@@ -418,7 +418,7 @@ CONFIG_PREALLOC_TIMERS=50
 # Stack and heap information
 #
 CONFIG_IDLETHREAD_STACKSIZE=4096
-CONFIG_USERMAIN_STACKSIZE=4096
+CONFIG_USERMAIN_STACKSIZE=3500
 CONFIG_PTHREAD_STACK_MIN=512
 CONFIG_PTHREAD_STACK_DEFAULT=2048
 

From 8630d82ea2b27389b686fe191e9b2f2fbd44f235 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 10:46:10 +0200
Subject: [PATCH 18/34] gps driver: Use correct spawn command, use a smaller
 start tool size

---
 src/drivers/gps/gps.cpp   | 12 ++++--------
 src/drivers/gps/module.mk |  2 ++
 2 files changed, 6 insertions(+), 8 deletions(-)

diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp
index 2d8d37298e..6195cd6eab 100644
--- a/src/drivers/gps/gps.cpp
+++ b/src/drivers/gps/gps.cpp
@@ -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
@@ -56,6 +56,7 @@
 #include <arch/board/board.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/device/i2c.h>
+#include <systemlib/systemlib.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/scheduling_priorities.h>
 #include <systemlib/err.h>
@@ -78,12 +79,6 @@
 #endif
 static const int ERROR = -1;
 
-#ifndef CONFIG_SCHED_WORKQUEUE
-# error This requires CONFIG_SCHED_WORKQUEUE.
-#endif
-
-
-
 class GPS : public device::CDev
 {
 public:
@@ -211,7 +206,8 @@ GPS::init()
 		goto out;
 
 	/* start the GPS driver worker task */
-	_task = task_create("gps", SCHED_PRIORITY_SLOW_DRIVER, 2048, (main_t)&GPS::task_main_trampoline, nullptr);
+	_task = task_spawn_cmd("gps", SCHED_DEFAULT,
+				SCHED_PRIORITY_SLOW_DRIVER, 2000, (main_t)&GPS::task_main_trampoline, nullptr);
 
 	if (_task < 0) {
 		warnx("task start failed: %d", errno);
diff --git a/src/drivers/gps/module.mk b/src/drivers/gps/module.mk
index 82c67d40a3..eb382c4b26 100644
--- a/src/drivers/gps/module.mk
+++ b/src/drivers/gps/module.mk
@@ -41,3 +41,5 @@ SRCS		= gps.cpp \
 		  gps_helper.cpp \
 		  mtk.cpp \
 		  ubx.cpp
+
+MODULE_STACKSIZE = 1200

From 9d2bfb596cd312c4de185f1932cb17f743deecd9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 10:46:47 +0200
Subject: [PATCH 19/34] flow example: Use smaller stack and correct start logic

---
 .../flow_position_estimator/flow_position_estimator_main.c   | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c
index 495c415f2b..c775428ef1 100644
--- a/src/examples/flow_position_estimator/flow_position_estimator_main.c
+++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c
@@ -65,6 +65,7 @@
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/filtered_bottom_flow.h>
 #include <systemlib/perf_counter.h>
+#include <systemlib/systemlib.h>
 #include <poll.h>
 
 #include "flow_position_estimator_params.h"
@@ -109,9 +110,9 @@ int flow_position_estimator_main(int argc, char *argv[])
 
 		thread_should_exit = false;
 		daemon_task = task_spawn_cmd("flow_position_estimator",
-					 SCHED_RR,
+					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 5,
-					 4096,
+					 4000,
 					 flow_position_estimator_thread_main,
 					 (argv) ? (const char **)&argv[2] : (const char **)NULL);
 		exit(0);

From cccd3e1dc47968cbe5d351bc327502196988215e Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 10:47:18 +0200
Subject: [PATCH 20/34] mavlink app: Reduce stack sizes minimally after further
 inspection

---
 src/modules/mavlink/mavlink_main.cpp     | 2 +-
 src/modules/mavlink/mavlink_receiver.cpp | 2 +-
 2 files changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp
index 199e85305c..6c97bfca71 100644
--- a/src/modules/mavlink/mavlink_main.cpp
+++ b/src/modules/mavlink/mavlink_main.cpp
@@ -2204,7 +2204,7 @@ Mavlink::start(int argc, char *argv[])
 	task_spawn_cmd(buf,
 		       SCHED_DEFAULT,
 		       SCHED_PRIORITY_DEFAULT,
-		       2000,
+		       1950,
 		       (main_t)&Mavlink::start_helper,
 		       (const char **)argv);
 
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index b03a68c076..72b9ee83a3 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -949,7 +949,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);

From 834a230fcf5d24cacaa16b2a0bb3add2a56ebb46 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 10:47:41 +0200
Subject: [PATCH 21/34] inav: Reduce stack size of start tool

---
 src/modules/position_estimator_inav/module.mk | 2 ++
 1 file changed, 2 insertions(+)

diff --git a/src/modules/position_estimator_inav/module.mk b/src/modules/position_estimator_inav/module.mk
index 939d768495..0658d3f09c 100644
--- a/src/modules/position_estimator_inav/module.mk
+++ b/src/modules/position_estimator_inav/module.mk
@@ -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

From 30123c8f0bad29c53d1840458c90be35411237d9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 10:48:15 +0200
Subject: [PATCH 22/34] inav: Fix scheduling type, we want ALL processes to
 stick to DEFAULT

---
 .../position_estimator_inav/position_estimator_inav_main.c      | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index 368424853c..d8d0ff37d3 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -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);

From ea99fd84464ba6141f182dece0ae7a60c3ea9377 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 16 May 2014 10:48:41 +0200
Subject: [PATCH 23/34] nshterm: Reduce stack size further after more thorough
 inspection

---
 src/systemcmds/nshterm/module.mk | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/systemcmds/nshterm/module.mk b/src/systemcmds/nshterm/module.mk
index fe79ec3b76..b22b446dae 100644
--- a/src/systemcmds/nshterm/module.mk
+++ b/src/systemcmds/nshterm/module.mk
@@ -38,4 +38,4 @@
 MODULE_COMMAND	 = nshterm
 SRCS		 = nshterm.c
 
-MODULE_STACKSIZE = 1500
+MODULE_STACKSIZE = 1200

From 692e8f84a93a932986004d896554a70380ea11e9 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 16 May 2014 22:12:07 +0200
Subject: [PATCH 24/34] commander: don't require good EPH for
 local_position_valid, position_estimator_inav: estimate EPH/EPV and publish
 it in local position topic

---
 src/modules/commander/commander.cpp           | 21 ++++++++++++++++++-
 .../position_estimator_inav_main.c            | 21 +++++++++++++++----
 src/modules/sdlog2/sdlog2.c                   | 10 +++++++--
 src/modules/sdlog2/sdlog2_messages.h          |  7 ++++---
 .../uORB/topics/vehicle_local_position.h      |  2 ++
 5 files changed, 51 insertions(+), 10 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 13da27dcd0..e4ae357d70 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1012,7 +1012,26 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* update condition_local_position_valid and condition_local_altitude_valid */
-		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && eph_epv_good, &(status.condition_local_position_valid), &status_changed);
+		/* hysteresis for EPH */
+		bool local_eph_good;
+
+		if (status.condition_global_position_valid) {
+			if (local_position.eph > eph_epv_threshold * 2.0f) {
+				local_eph_good = false;
+
+			} else {
+				local_eph_good = true;
+			}
+
+		} else {
+			if (local_position.eph < eph_epv_threshold) {
+				local_eph_good = true;
+
+			} else {
+				local_eph_good = false;
+			}
+		}
+		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.xy_valid && local_eph_good, &(status.condition_local_position_valid), &status_changed);
 		check_valid(local_position.timestamp, POSITION_TIMEOUT, local_position.z_valid, &(status.condition_local_altitude_valid), &status_changed);
 
 		static bool published_condition_landed_fw = false;
diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c
index d8d0ff37d3..fdc3233e03 100644
--- a/src/modules/position_estimator_inav/position_estimator_inav_main.c
+++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c
@@ -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_m);
+						epv = fminf(epv, gps.epv_m);
+
 						w_gps_xy = min_eph_epv / fmaxf(min_eph_epv, gps.eph_m);
 						w_gps_z = min_eph_epv / fmaxf(min_eph_epv, gps.epv_m);
 					}
@@ -712,6 +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_m;
-				global_pos.epv = gps.epv_m;
+				global_pos.eph = eph;
+				global_pos.epv = epv;
 
 				if (vehicle_global_position_pub < 0) {
 					vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 39f433eb53..70ce768068 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1119,10 +1119,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);
 		}
 
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 0c61886579..90025b9ff6 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -109,10 +109,11 @@ struct log_LPOS_s {
 	int32_t ref_lat;
 	int32_t ref_lon;
 	float ref_alt;
-	uint8_t xy_flags;
-	uint8_t z_flags;
+	uint8_t pos_flags;
 	uint8_t landed;
 	uint8_t ground_dist_flags;
+	float eph;
+	float epv;
 };
 
 /* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -360,7 +361,7 @@ 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(ATTC, "ffff",		"Roll,Pitch,Yaw,Thrust"),
diff --git a/src/modules/uORB/topics/vehicle_local_position.h b/src/modules/uORB/topics/vehicle_local_position.h
index a15303ea48..5d39c897d4 100644
--- a/src/modules/uORB/topics/vehicle_local_position.h
+++ b/src/modules/uORB/topics/vehicle_local_position.h
@@ -83,6 +83,8 @@ struct vehicle_local_position_s {
 	float dist_bottom_rate;		/**< Distance to bottom surface (ground) change rate */
 	uint64_t surface_bottom_timestamp;		/**< Time when new bottom surface found */
 	bool dist_bottom_valid;	/**< true if distance to bottom surface is valid */
+	float eph;
+	float epv;
 };
 
 /**

From f52edc4cdee0aec037d5def8ee805bb1904bb6f9 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sat, 17 May 2014 09:18:54 +0200
Subject: [PATCH 25/34] ekf_att_pos_estimator: fixed files names and perf
 counters names

---
 ...r_main.cpp => ekf_att_pos_estimator_main.cpp} | 16 ++++++++--------
 ...r_params.c => ekf_att_pos_estimator_params.c} |  2 +-
 2 files changed, 9 insertions(+), 9 deletions(-)
 rename src/modules/ekf_att_pos_estimator/{fw_att_pos_estimator_main.cpp => ekf_att_pos_estimator_main.cpp} (98%)
 rename src/modules/ekf_att_pos_estimator/{fw_att_pos_estimator_params.c => ekf_att_pos_estimator_params.c} (99%)

diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
similarity index 98%
rename from src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
rename to src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
index 907f4c2e1d..7a71894ed8 100644
--- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file fw_att_pos_estimator_main.cpp
+ * @file ekf_att_pos_estimator_main.cpp
  * Implementation of the attitude and position estimator.
  *
  * @author Paul Riseborough <p_riseborough@live.com.au>
@@ -336,13 +336,13 @@ FixedwingEstimator::FixedwingEstimator() :
 	_baro_gps_offset(0.0f),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_COUNT, "fw_att_pos_estimator")),
-	_perf_gyro(perf_alloc(PC_COUNT, "fw_ekf_gyro_upd")),
-	_perf_accel(perf_alloc(PC_COUNT, "fw_ekf_accel_upd")),
-	_perf_mag(perf_alloc(PC_COUNT, "fw_ekf_mag_upd")),
-	_perf_gps(perf_alloc(PC_COUNT, "fw_ekf_gps_upd")),
-	_perf_baro(perf_alloc(PC_COUNT, "fw_ekf_baro_upd")),
-	_perf_airspeed(perf_alloc(PC_COUNT, "fw_ekf_aspd_upd")),
+	_loop_perf(perf_alloc(PC_COUNT, "ekf_att_pos_estimator")),
+	_perf_gyro(perf_alloc(PC_COUNT, "ekf_att_pos_gyro_upd")),
+	_perf_accel(perf_alloc(PC_COUNT, "ekf_att_pos_accel_upd")),
+	_perf_mag(perf_alloc(PC_COUNT, "ekf_att_pos_mag_upd")),
+	_perf_gps(perf_alloc(PC_COUNT, "ekf_att_pos_gps_upd")),
+	_perf_baro(perf_alloc(PC_COUNT, "ekf_att_pos_baro_upd")),
+	_perf_airspeed(perf_alloc(PC_COUNT, "ekf_att_pos_aspd_upd")),
 
 /* states */
 	_initialized(false),
diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
similarity index 99%
rename from src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c
rename to src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
index d2c6e1f15e..1d9ae46236 100644
--- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c
+++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_params.c
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file fw_att_pos_estimator_params.c
+ * @file ekf_att_pos_estimator_params.c
  *
  * Parameters defined by the attitude and position estimator task
  *

From 98fcec243bac42507aa4469bd79831f847e2e846 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sat, 17 May 2014 09:21:47 +0200
Subject: [PATCH 26/34] ekf_att_pos_estimator: module.mk fixed

---
 src/modules/ekf_att_pos_estimator/module.mk | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk
index 30955d0ddc..6fefec2c2a 100644
--- a/src/modules/ekf_att_pos_estimator/module.mk
+++ b/src/modules/ekf_att_pos_estimator/module.mk
@@ -32,11 +32,11 @@
 ############################################################################
 
 #
-# Main Attitude and Position Estimator for Fixed Wing Aircraft
+# Main EKF Attitude and Position Estimator
 #
 
 MODULE_COMMAND	= ekf_att_pos_estimator
 
-SRCS		= fw_att_pos_estimator_main.cpp \
-		  fw_att_pos_estimator_params.c \
+SRCS		= ekf_att_pos_estimator_main.cpp \
+		  ekf_att_pos_estimator_params.c \
 		  estimator.cpp

From 32ae2dd1d094f4554f5acdad8fb76ed9eb3ba1f0 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sat, 17 May 2014 11:00:14 +0200
Subject: [PATCH 27/34] commander: missed 'break' in 'switch' added

---
 src/modules/commander/commander.cpp | 1 +
 1 file changed, 1 insertion(+)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 13da27dcd0..504696ff94 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1710,6 +1710,7 @@ set_control_mode()
 
 		case MAIN_STATE_AUTO:
 			navigator_enabled = true;
+			break;
 
 		default:
 			break;

From ce62f073796f5956b32eaa8eb8d4a371821077de Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 19 May 2014 21:06:32 +0200
Subject: [PATCH 28/34] Fix EPH / EPV conversion for MediaTek units

---
 src/drivers/gps/mtk.cpp | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index c90ecbe281..60a98134db 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -253,8 +253,8 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 
 	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm
 	_gps_position->fix_type = packet.fix_type;
-	_gps_position->eph_m = packet.hdop; // XXX: Check this because eph_m is in m and hdop is without unit
-	_gps_position->epv_m = 0.0; //unknown in mtk custom mode
+	_gps_position->eph_m = packet.hdop / 100.0f; // from cm to m
+	_gps_position->epv_m = _gps_position->eph_m; // unknown in mtk custom mode, so we cheat with eph
 	_gps_position->vel_m_s = ((float)packet.ground_speed) * 1e-2f; // from cm/s to m/s
 	_gps_position->cog_rad = ((float)packet.heading) * M_DEG_TO_RAD_F * 1e-2f; //from deg *100 to rad
 	_gps_position->satellites_visible = packet.satellites;

From f0630547aa6d391fbb056e450d2344bd888721e2 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 19 May 2014 21:07:19 +0200
Subject: [PATCH 29/34] MTK: Bail out correctly

---
 src/drivers/gps/mtk.cpp | 6 ++++++
 1 file changed, 6 insertions(+)

diff --git a/src/drivers/gps/mtk.cpp b/src/drivers/gps/mtk.cpp
index 60a98134db..680f00d97f 100644
--- a/src/drivers/gps/mtk.cpp
+++ b/src/drivers/gps/mtk.cpp
@@ -249,6 +249,12 @@ MTK::handle_message(gps_mtk_packet_t &packet)
 		warnx("mtk: unknown revision");
 		_gps_position->lat = 0;
 		_gps_position->lon = 0;
+
+		// Indicate this data is not usable and bail out
+		_gps_position->eph_m = 1000.0f;
+		_gps_position->epv_m = 1000.0f;
+		_gps_position->fix_type = 0;
+		return;
 	}
 
 	_gps_position->alt = (int32_t)(packet.msl_altitude * 10); // from cm to mm

From 56a8f3de0a518d189707ef0da062325ab797f4a9 Mon Sep 17 00:00:00 2001
From: John Hiesey <john@hiesey.com>
Date: Tue, 20 May 2014 00:47:48 -0700
Subject: [PATCH 30/34] Add mixer config for hexa coax frame

---
 ROMFS/px4fmu_common/init.d/11001_hexa_cox        |  2 --
 src/modules/systemlib/mixer/mixer.h              |  1 +
 src/modules/systemlib/mixer/mixer_multirotor.cpp | 13 +++++++++++++
 src/modules/systemlib/mixer/multi_tables         | 15 ++++++++++++---
 4 files changed, 26 insertions(+), 5 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/11001_hexa_cox b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
index df2e609bc8..daa04a4de8 100644
--- a/ROMFS/px4fmu_common/init.d/11001_hexa_cox
+++ b/ROMFS/px4fmu_common/init.d/11001_hexa_cox
@@ -1,7 +1,5 @@
 #!nsh
 #
-# UNTESTED UNTESTED!
-#
 # Generic 10" Hexa coaxial geometry
 #
 # Lorenz Meier <lm@inf.ethz.ch>
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index fa9d28b74c..225570fa40 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -447,6 +447,7 @@ public:
 		QUAD_WIDE,	/**< quad in wide configuration */
 		HEX_X,		/**< hex in X configuration */
 		HEX_PLUS,	/**< hex in + configuration */
+		HEX_COX,
 		OCTA_X,
 		OCTA_PLUS,
 		OCTA_COX,
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index 1ca0a21e97..4ad21d8188 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -115,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 },
@@ -152,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],
@@ -163,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 */
@@ -252,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;
 
diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables
index 050bf2f47f..b5698036e1 100755
--- a/src/modules/systemlib/mixer/multi_tables
+++ b/src/modules/systemlib/mixer/multi_tables
@@ -52,6 +52,15 @@ set hex_plus {
 	 120 CW
 }
 
+set hex_cox {
+	  60 CW
+	  60 CCW
+	 180 CW
+	 180 CCW
+	 -60 CW
+	 -60 CCW
+}
+
 set octa_x {
 	  22.5 CW
 	-157.5 CW
@@ -85,7 +94,7 @@ set octa_cox {
 	-135 CW
 }
 
-set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus octa_cox}
+set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus hex_cox octa_x octa_plus octa_cox}
 
 proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
 
@@ -104,13 +113,13 @@ foreach table $tables {
 	puts "};"
 }
 
-puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {"
+puts "const MultirotorMixer::Rotor *_config_index\[MultirotorMixer::MAX_GEOMETRY\] = {"
 foreach table $tables {
 	puts [format "\t&_config_%s\[0\]," $table]
 }
 puts "};"
 
-puts "const unsigned _config_rotor_count\[MultirotorMixer::Geometry::MAX_GEOMETRY\] = {"
+puts "const unsigned _config_rotor_count\[MultirotorMixer::MAX_GEOMETRY\] = {"
 foreach table $tables {
 	upvar #0 $table angles
 	puts [format "\t%u, /* %s */" [expr [llength $angles] / 2] $table]

From 4a666d094d016527bcb342e6209fb3ad40f0ed58 Mon Sep 17 00:00:00 2001
From: John Hiesey <john@hiesey.com>
Date: Tue, 20 May 2014 01:25:52 -0700
Subject: [PATCH 31/34] frsky_telemetry: fix gps data format

Eliminate inadvertent rounding and generate degrees/minutes
instead of degrees/minutes/seconds output per frsky docs
---
 src/drivers/frsky_telemetry/frsky_data.c | 15 +++++----------
 1 file changed, 5 insertions(+), 10 deletions(-)

diff --git a/src/drivers/frsky_telemetry/frsky_data.c b/src/drivers/frsky_telemetry/frsky_data.c
index 18de3f4dac..dd9b90fb33 100644
--- a/src/drivers/frsky_telemetry/frsky_data.c
+++ b/src/drivers/frsky_telemetry/frsky_data.c
@@ -194,17 +194,12 @@ void frsky_send_frame1(int uart)
 }
 
 /**
- * Formats the decimal latitude/longitude to the required degrees/minutes/seconds.
+ * Formats the decimal latitude/longitude to the required degrees/minutes.
  */
 static float frsky_format_gps(float dec)
 {
-	float dms_deg = (int) dec;
-	float dec_deg = dec - dms_deg;
-	float dms_min = (int) (dec_deg * 60);
-	float dec_min = (dec_deg * 60) - dms_min;
-	float dms_sec = dec_min * 60;
-
-	return (dms_deg * 100.0f) + dms_min + (dms_sec / 100.0f);
+	float dm_deg = (int) dec;
+	return (dm_deg * 100.0f) + (dec - dm_deg) * 60;
 }
 
 /**
@@ -232,9 +227,9 @@ void frsky_send_frame2(int uart)
 		struct tm *tm_gps = gmtime(&time_gps);
 
 		course = (global_pos.yaw + M_PI_F) / M_PI_F * 180.0f;
-		lat    = frsky_format_gps(abs(global_pos.lat));
+		lat    = frsky_format_gps(fabsf(global_pos.lat));
 		lat_ns = (global_pos.lat < 0) ? 'S' : 'N';
-		lon    = frsky_format_gps(abs(global_pos.lon));
+		lon    = frsky_format_gps(fabsf(global_pos.lon));
 		lon_ew = (global_pos.lon < 0) ? 'W' : 'E';
 		speed  = sqrtf(global_pos.vel_n * global_pos.vel_n + global_pos.vel_e * global_pos.vel_e)
 				* 25.0f / 46.0f;

From d8ef397b07280a2631076c2f92b950d64684a8ed Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 20 May 2014 16:40:44 +0200
Subject: [PATCH 32/34] mc_att_control: reset yaw setpoint after ACRO

---
 src/modules/mc_att_control/mc_att_control_main.cpp | 3 +++
 1 file changed, 3 insertions(+)

diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp
index 77c7c61e9b..20e016da34 100644
--- a/src/modules/mc_att_control/mc_att_control_main.cpp
+++ b/src/modules/mc_att_control/mc_att_control_main.cpp
@@ -803,6 +803,9 @@ MulticopterAttitudeControl::task_main()
 					_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);

From 559bfbb11cdac499cda8070c32e2b7e79b3b883e Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 20 May 2014 16:42:11 +0200
Subject: [PATCH 33/34] commander: allow disarm in ACRO mode

---
 src/modules/commander/commander.cpp | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 8336bcf329..45ef79d7c5 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -1192,7 +1192,7 @@ int commander_thread_main(int argc, char *argv[])
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
 			    (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) &&
-			    (status.main_state == MAIN_STATE_MANUAL || status.condition_landed) &&
+			    (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) &&
 			    sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {

From fb801b6faecd77fe2aac54d3389cacf73993ccc4 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Tue, 20 May 2014 21:46:42 +0200
Subject: [PATCH 34/34] mavlink: minor fix

---
 src/modules/mavlink/mavlink_messages.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 2837c16b80..933478f560 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -140,7 +140,6 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 		} 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;
-			custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY;
 		}
 
 	} else {