From f4c36f8c5cf69bd3af55a1065ab7a18c999c054c Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 1 May 2014 20:57:36 +0200
Subject: [PATCH 01/36] mc_pos_control: use current velocity to calculate
 position setpoint on reset to make transition to stabilized modes more smooth

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 6 +++---
 1 file changed, 3 insertions(+), 3 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 7c625a0c53..fe0e8fe1ae 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -493,8 +493,8 @@ MulticopterPositionControl::reset_pos_sp()
 {
 	if (_reset_pos_sp) {
 		_reset_pos_sp = false;
-		_pos_sp(0) = _pos(0);
-		_pos_sp(1) = _pos(1);
+		_pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0);
+		_pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
 	}
 }
@@ -504,7 +504,7 @@ MulticopterPositionControl::reset_alt_sp()
 {
 	if (_reset_alt_sp) {
 		_reset_alt_sp = false;
-		_pos_sp(2) = _pos(2);
+		_pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
 	}
 }

From 7232c0354bee1f8f572bdb2f53f9969ba9778a6e Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Thu, 1 May 2014 23:01:30 +0200
Subject: [PATCH 02/36] mc_pos_control: use MPC_XXX_FF to adjust setpoint on
 reset

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 8 +++++---
 1 file changed, 5 insertions(+), 3 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index fe0e8fe1ae..356f3c15e8 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -388,9 +388,11 @@ MulticopterPositionControl::parameters_update(bool force)
 		param_get(_params_handles.z_vel_max, &v);
 		_params.vel_max(2) = v;
 		param_get(_params_handles.xy_ff, &v);
+		v = math::constrain(v, 0.0f, 1.0f);
 		_params.vel_ff(0) = v;
 		_params.vel_ff(1) = v;
 		param_get(_params_handles.z_ff, &v);
+		v = math::constrain(v, 0.0f, 1.0f);
 		_params.vel_ff(2) = v;
 
 		_params.sp_offs_max = _params.vel_max.edivide(_params.pos_p) * 2.0f;
@@ -493,8 +495,8 @@ MulticopterPositionControl::reset_pos_sp()
 {
 	if (_reset_pos_sp) {
 		_reset_pos_sp = false;
-		_pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0);
-		_pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1);
+		_pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0) * (1.0f - _params.vel_ff(0));
+		_pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1) * (1.0f - _params.vel_ff(1));
 		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
 	}
 }
@@ -504,7 +506,7 @@ MulticopterPositionControl::reset_alt_sp()
 {
 	if (_reset_alt_sp) {
 		_reset_alt_sp = false;
-		_pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2);
+		_pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2) * (1.0f - _params.vel_ff(2));
 		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
 	}
 }

From 9d7e3feffcfb34708cda5a726b7900101f41b205 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 31 Jul 2014 11:18:59 +0200
Subject: [PATCH 03/36] airspeed: Filter slightly more, just a touch to not
 induce phase delay

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

diff --git a/src/drivers/meas_airspeed/meas_airspeed.cpp b/src/drivers/meas_airspeed/meas_airspeed.cpp
index 9cfa7f3838..1597062789 100644
--- a/src/drivers/meas_airspeed/meas_airspeed.cpp
+++ b/src/drivers/meas_airspeed/meas_airspeed.cpp
@@ -103,7 +103,7 @@
 
 /* Measurement rate is 100Hz */
 #define MEAS_RATE 100
-#define MEAS_DRIVER_FILTER_FREQ 1.5f
+#define MEAS_DRIVER_FILTER_FREQ 1.2f
 #define CONVERSION_INTERVAL	(1000000 / MEAS_RATE)	/* microseconds */
 
 class MEASAirspeed : public Airspeed

From fc2e0fad4731ef543be4c3da73de6b670d40d804 Mon Sep 17 00:00:00 2001
From: philipoe <philipp.oettershagen@mavt.ethz.ch>
Date: Thu, 31 Jul 2014 15:23:30 +0200
Subject: [PATCH 04/36] TECS: Added separate gain / time constant for the
 throttle loop to allow independent tuning

---
 src/lib/external_lgpl/tecs/tecs.cpp | 2 +-
 src/lib/external_lgpl/tecs/tecs.h   | 5 +++++
 2 files changed, 6 insertions(+), 1 deletion(-)

diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 6386e37a03..16c7e5ffad 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -298,7 +298,7 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 
 	} else {
 		// Calculate gain scaler from specific energy error to throttle
-		float K_STE2Thr = 1 / (_timeConst * (_STEdot_max - _STEdot_min));
+		float K_STE2Thr = 1 / (_timeConstThrot * (_STEdot_max - _STEdot_min));
 
 		// Calculate feed-forward throttle
 		float ff_throttle = 0;
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 5cafb1c791..10c9e9344f 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -123,6 +123,10 @@ public:
 		_timeConst = time_const;
 	}
 
+	void set_time_const_throt(float time_const_throt) {
+		_timeConstThrot = time_const_throt;
+	}
+
 	void set_min_sink_rate(float rate) {
 		_minSinkRate = rate;
 	}
@@ -204,6 +208,7 @@ private:
 	float _minSinkRate;
 	float _maxSinkRate;
 	float _timeConst;
+	float _timeConstThrot;
 	float _ptchDamp;
 	float _thrDamp;
 	float _integGain;

From 8e33278c4cfc979e6ca69994186b3a6144847d6a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Aug 2014 13:04:28 +0200
Subject: [PATCH 05/36] Move the last throttle demand setting to a better
 position

---
 src/lib/external_lgpl/tecs/tecs.cpp | 5 ++++-
 1 file changed, 4 insertions(+), 1 deletion(-)

diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 16c7e5ffad..579fb5d42c 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -327,9 +327,12 @@ void TECS::_update_throttle(float throttle_cruise, const math::Matrix<3,3> &rotM
 			_throttle_dem = constrain(_throttle_dem,
 						  _last_throttle_dem - thrRateIncr,
 						  _last_throttle_dem + thrRateIncr);
-			_last_throttle_dem = _throttle_dem;
 		}
 
+		// Ensure _last_throttle_dem is always initialized properly
+		// Also: The throttle_slewrate limit is only applied to throttle_dem, but does not limit the integrator!!
+		_last_throttle_dem = _throttle_dem;
+
 
 		// Calculate integrator state upper and lower limits
 		// Set to a value thqat will allow 0.1 (10%) throttle saturation to allow for noise on the demand

From 3b1c92e42f57277a12d40963c22fbea9820ee0ad Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Aug 2014 13:10:07 +0200
Subject: [PATCH 06/36] Make throttle time constant tunable separately, group
 TECS params correctly, make climbout alt configurable

---
 .../fw_pos_control_l1_main.cpp                | 11 ++++-
 .../fw_pos_control_l1_params.c                | 49 ++++++++++++++-----
 2 files changed, 46 insertions(+), 14 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index eadb63f40d..f05f20cd00 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -203,9 +203,11 @@ private:
 		float l1_damping;
 
 		float time_const;
+		float time_const_throt;
 		float min_sink_rate;
 		float max_sink_rate;
 		float max_climb_rate;
+		float climbout_diff;
 		float heightrate_p;
 		float speedrate_p;
 		float throttle_damp;
@@ -245,9 +247,11 @@ private:
 		param_t l1_damping;
 
 		param_t time_const;
+		param_t time_const_throt;
 		param_t min_sink_rate;
 		param_t max_sink_rate;
 		param_t max_climb_rate;
+		param_t climbout_diff;
 		param_t heightrate_p;
 		param_t speedrate_p;
 		param_t throttle_damp;
@@ -471,9 +475,11 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT");
 
 	_parameter_handles.time_const = 			param_find("FW_T_TIME_CONST");
+	_parameter_handles.time_const_throt = 			param_find("FW_T_THRO_CONST");
 	_parameter_handles.min_sink_rate = 			param_find("FW_T_SINK_MIN");
 	_parameter_handles.max_sink_rate =			param_find("FW_T_SINK_MAX");
 	_parameter_handles.max_climb_rate =			param_find("FW_T_CLMB_MAX");
+	_parameter_handles.climbout_diff =			param_find("FW_CLMBOUT_DIFF");
 	_parameter_handles.throttle_damp = 			param_find("FW_T_THR_DAMP");
 	_parameter_handles.integrator_gain =			param_find("FW_T_INTEG_GAIN");
 	_parameter_handles.vertical_accel_limit =		param_find("FW_T_VERT_ACC");
@@ -536,6 +542,7 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
 
 	param_get(_parameter_handles.time_const, &(_parameters.time_const));
+	param_get(_parameter_handles.time_const_throt, &(_parameters.time_const_throt));
 	param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate));
 	param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate));
 	param_get(_parameter_handles.throttle_damp, &(_parameters.throttle_damp));
@@ -547,6 +554,7 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.speed_weight, &(_parameters.speed_weight));
 	param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping));
 	param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate));
+	param_get(_parameter_handles.climbout_diff, &(_parameters.climbout_diff));
 
 	param_get(_parameter_handles.heightrate_p, &(_parameters.heightrate_p));
 	param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
@@ -564,6 +572,7 @@ FixedwingPositionControl::parameters_update()
 	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
 
 	_tecs.set_time_const(_parameters.time_const);
+	_tecs.set_time_const_throt(_parameters.time_const_throt);
 	_tecs.set_min_sink_rate(_parameters.min_sink_rate);
 	_tecs.set_max_sink_rate(_parameters.max_sink_rate);
 	_tecs.set_throttle_damp(_parameters.throttle_damp);
@@ -1095,7 +1104,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				usePreTakeoffThrust = false;
 
 				/* apply minimum pitch and limit roll if target altitude is not within 10 meters */
-				if (altitude_error > 15.0f) {
+				if (_parameters.climbout_diff > 0.001f && altitude_error > _parameters.climbout_diff) {
 
 					/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
 					tecs_update_pitch_throttle(_pos_sp_triplet.current.alt,
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index 52128e1b7a..a0d9309b92 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -154,6 +154,18 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);
  */
 PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f);
 
+/**
+ * Climbout Altitude difference
+ *
+ * If the altitude error exceeds this parameter, the system will climb out
+ * with maximum throttle and minimum airspeed until it is closer than this
+ * distance to the desired altitude. Mostly used for takeoff waypoints / modes.
+ * Set to zero to disable climbout mode (not recommended).
+ *
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_CLMBOUT_DIFF, 25.0f);
+
 /**
  * Maximum climb rate
  *
@@ -181,7 +193,7 @@ PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f);
  * set to THR_MIN and flown at the same airspeed as used 
  * to measure FW_T_CLMB_MAX.
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
 
@@ -194,7 +206,7 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MIN, 2.0f);
  * exceeding the lower pitch angle limit and without over-speeding 
  * the aircraft.
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
 
@@ -205,17 +217,28 @@ PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
  * Smaller values make it faster to respond, larger values make it slower
  * to respond.
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_TIME_CONST, 5.0f);
 
+/**
+ * TECS Throttle time constant
+ *
+ * This is the time constant of the TECS throttle control algorithm (in seconds). 
+ * Smaller values make it faster to respond, larger values make it slower
+ * to respond.
+ *
+ * @group Fixed Wing TECS
+ */
+PARAM_DEFINE_FLOAT(FW_T_THRO_CONST, 8.0f);
+
 /**
  * Throttle damping factor
  *
  * This is the damping gain for the throttle demand loop. 
  * Increase to add damping to correct for oscillations in speed and height.
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
 
@@ -227,7 +250,7 @@ PARAM_DEFINE_FLOAT(FW_T_THR_DAMP, 0.5f);
  * and height offsets are trimmed out, but reduces damping and 
  * increases overshoot.
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
 
@@ -240,7 +263,7 @@ PARAM_DEFINE_FLOAT(FW_T_INTEG_GAIN, 0.1f);
  * allows for reasonably aggressive pitch changes if required to recover 
  * from under-speed conditions.
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
 
@@ -253,7 +276,7 @@ PARAM_DEFINE_FLOAT(FW_T_VERT_ACC, 7.0f);
  * the solution more towards use of the barometer, whilst reducing it weights 
  * the solution more towards use of the accelerometer data.
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
 
@@ -266,7 +289,7 @@ PARAM_DEFINE_FLOAT(FW_T_HGT_OMEGA, 3.0f);
  * more towards use of the arispeed sensor, whilst reducing it weights the 
  * solution more towards use of the accelerometer data.
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
 
@@ -282,7 +305,7 @@ PARAM_DEFINE_FLOAT(FW_T_SPD_OMEGA, 2.0f);
  * aircraft (eg powered sailplanes) can use a lower value, whereas 
  * inefficient low aspect-ratio models (eg delta wings) can use a higher value.
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
 
@@ -300,7 +323,7 @@ PARAM_DEFINE_FLOAT(FW_T_RLL2THR, 10.0f);
  * Note to Glider Pilots - set this parameter to 2.0 (The glider will 
  * adjust its pitch angle to maintain airspeed, ignoring changes in height).
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
 
@@ -312,21 +335,21 @@ PARAM_DEFINE_FLOAT(FW_T_SPDWEIGHT, 1.0f);
  * will work well provided the pitch to servo controller has been tuned 
  * properly.
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
 
 /**
  * Height rate P factor
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_HRATE_P, 0.05f);
 
 /**
  * Speed rate P factor
  *
- * @group L1 Control
+ * @group Fixed Wing TECS
  */
 PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
 

From 2e4dff3fea2177538f3549cedd8f84fbbbecd4ad Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Aug 2014 13:21:30 +0200
Subject: [PATCH 07/36] removed debug statement from mTECS

---
 src/modules/fw_pos_control_l1/mtecs/mTecs.cpp | 1 -
 1 file changed, 1 deletion(-)

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index afc411a60a..749f57a2ba 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -76,7 +76,6 @@ mTecs::mTecs() :
 	_counter(0),
 	_debug(false)
 {
-	warnx("starting");
 }
 
 mTecs::~mTecs()

From 32e32d92bdad3eb4e92e9e53c112fcdcf848c7e6 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Aug 2014 14:39:24 +0200
Subject: [PATCH 08/36] Add throttle slew rate param

---
 .../fw_pos_control_l1/fw_pos_control_l1_main.cpp      |  5 +++++
 .../fw_pos_control_l1/fw_pos_control_l1_params.c      | 11 +++++++++++
 2 files changed, 16 insertions(+)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index f05f20cd00..189a19d48f 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -229,6 +229,7 @@ private:
 		float throttle_min;
 		float throttle_max;
 		float throttle_cruise;
+		float throttle_slew_max;
 
 		float throttle_land_max;
 
@@ -273,6 +274,7 @@ private:
 		param_t throttle_min;
 		param_t throttle_max;
 		param_t throttle_cruise;
+		param_t throttle_slew_max;
 
 		param_t throttle_land_max;
 
@@ -464,6 +466,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.roll_limit = param_find("FW_R_LIM");
 	_parameter_handles.throttle_min = param_find("FW_THR_MIN");
 	_parameter_handles.throttle_max = param_find("FW_THR_MAX");
+	_parameter_handles.throttle_slew_max = param_find("FW_THR_SLEW_MAX");
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 
@@ -538,6 +541,7 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
 	param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
 	param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
+	param_get(_parameter_handles.throttle_slew_max, &(_parameters.throttle_slew_max));
 
 	param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max));
 
@@ -576,6 +580,7 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_min_sink_rate(_parameters.min_sink_rate);
 	_tecs.set_max_sink_rate(_parameters.max_sink_rate);
 	_tecs.set_throttle_damp(_parameters.throttle_damp);
+	_tecs.set_throttle_slewrate(_parameters.throttle_slew_max);
 	_tecs.set_integrator_gain(_parameters.integrator_gain);
 	_tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit);
 	_tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega);
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index a0d9309b92..41c3744076 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -82,6 +82,17 @@ PARAM_DEFINE_FLOAT(FW_L1_DAMPING, 0.75f);
  */
 PARAM_DEFINE_FLOAT(FW_THR_CRUISE, 0.7f);
 
+/**
+ * Throttle max slew rate
+ *
+ * Maximum slew rate for the commanded throttle
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group L1 Control
+ */
+PARAM_DEFINE_FLOAT(FW_THR_SLEW_MAX, 0.0f);
+
 /**
  * Negative pitch limit
  *

From 491c6c29acef343819f32655e6e87334531af8cf Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Aug 2014 14:43:56 +0200
Subject: [PATCH 09/36] Init values

---
 src/lib/external_lgpl/tecs/tecs.h | 20 ++++++++++++++++++++
 1 file changed, 20 insertions(+)

diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 10c9e9344f..752b0ddd9c 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -28,6 +28,26 @@ class __EXPORT TECS
 {
 public:
 	TECS() :
+		_update_50hz_last_usec(0),
+		_update_speed_last_usec(0),
+		_update_pitch_throttle_last_usec(0),
+		// TECS tuning parameters
+		_hgtCompFiltOmega(0.0f),
+		_spdCompFiltOmega(0.0f),
+		_maxClimbRate(2.0f),
+		_minSinkRate(1.0f),
+		_maxSinkRate(2.0f),
+		_timeConst(5.0f),
+		_timeConstThrot(8.0f),
+		_ptchDamp(0.0f),
+		_thrDamp(0.0f),
+		_integGain(0.0f),
+		_vertAccLim(0.0f),
+		_rollComp(0.0f),
+		_spdWeight(0.5f),
+		_heightrate_p(0.0f),
+		_speedrate_p(0.0f),
+		_throttle_dem(0.0f),
 		_pitch_dem(0.0f),
 		_integ1_state(0.0f),
 		_integ2_state(0.0f),

From c3666a5504fc89b7e1861e25c9b6cb0c07453784 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Aug 2014 23:18:43 +0200
Subject: [PATCH 10/36] Widen default first WP radius

---
 src/modules/navigator/mission_params.c | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c
index 881caa24e0..d15e1e771b 100644
--- a/src/modules/navigator/mission_params.c
+++ b/src/modules/navigator/mission_params.c
@@ -78,7 +78,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
  * waypoint is more distant than MIS_DIS_1WP from the current position.
  *
  * @min 0
- * @max 250
+ * @max 1000
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 175);
+PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);

From 4d03202c4c8c322f9d93da0051dca015ab473cde Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Fri, 1 Aug 2014 23:42:26 +0200
Subject: [PATCH 11/36] Warn on far waypoints

---
 src/modules/navigator/mission.cpp | 4 ++++
 1 file changed, 4 insertions(+)

diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp
index 12f6b9c211..c0e37a3eda 100644
--- a/src/modules/navigator/mission.cpp
+++ b/src/modules/navigator/mission.cpp
@@ -273,6 +273,10 @@ Mission::check_dist_1wp()
 
 						if (dist_to_1wp < _param_dist_1wp.get()) {
 							_dist_1wp_ok = true;
+							if (dist_to_1wp > ((_param_dist_1wp.get() * 3) / 2)) {
+								/* allow at 2/3 distance, but warn */
+								mavlink_log_critical(_navigator->get_mavlink_fd(), "Warning: First waypoint very far: %d m", (int)dist_to_1wp);
+							}
 							return true;
 
 						} else {

From 9725a1635254ba6c0c1df0413f94398ec28e23f9 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Sat, 2 Aug 2014 03:34:57 +0400
Subject: [PATCH 12/36] UAVCAN: Fixed short git hash computation

---
 src/modules/uavcan/uavcan_main.cpp | 17 +++++++----------
 1 file changed, 7 insertions(+), 10 deletions(-)

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index ffd5990708..9d5404bafe 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -180,16 +180,13 @@ void UavcanNode::fill_node_info()
 	/* software version */
 	uavcan::protocol::SoftwareVersion swver;
 
-	// Extracting the last 8 hex digits of FW_GIT and converting them to int
-	const unsigned fw_git_len = std::strlen(FW_GIT);
-	if (fw_git_len >= 8) {
-		char fw_git_short[9] = {};
-		std::memmove(fw_git_short, FW_GIT + fw_git_len - 8, 8);
-		assert(fw_git_short[8] == '\0');
-		char *end = nullptr;
-		swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
-		swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
-	}
+	// Extracting the first 8 hex digits of FW_GIT and converting them to int
+	char fw_git_short[9] = {};
+	std::memmove(fw_git_short, FW_GIT, 8);
+	assert(fw_git_short[8] == '\0');
+	char *end = nullptr;
+	swver.vcs_commit = std::strtol(fw_git_short, &end, 16);
+	swver.optional_field_mask |= swver.OPTIONAL_FIELD_MASK_VCS_COMMIT;
 
 	warnx("SW version vcs_commit: 0x%08x", unsigned(swver.vcs_commit));
 

From 5824607c76ed38b5c08d1c9031814f741bce7c8b Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Sat, 2 Aug 2014 13:01:42 +0200
Subject: [PATCH 13/36] uavcan: fix actuator groups subcriptions and poll()

---
 src/modules/uavcan/uavcan_main.cpp | 36 ++++++++++++++++--------------
 1 file changed, 19 insertions(+), 17 deletions(-)

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 9d5404bafe..5b1539e3ed 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -253,7 +253,6 @@ int UavcanNode::run()
 	// XXX figure out the output count
 	_output_count = 2;
 
-
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
 	actuator_outputs_s outputs;
@@ -273,21 +272,23 @@ int UavcanNode::run()
 
 	_node.setStatusOk();
 
-	while (!_task_should_exit) {
+	/*
+	 * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
+	 * Please note that with such multiplexing it is no longer possible to rely only on
+	 * the value returned from poll() to detect whether actuator control has timed out or not.
+	 * Instead, all ORB events need to be checked individually (see below).
+	 */
+	_poll_fds_num = 0;
+	_poll_fds[_poll_fds_num] = ::pollfd();
+	_poll_fds[_poll_fds_num].fd = busevent_fd;
+	_poll_fds[_poll_fds_num].events = POLLIN;
+	_poll_fds_num += 1;
 
+	while (!_task_should_exit) {
+		// update actuator controls subscriptions if needed
 		if (_groups_subscribed != _groups_required) {
 			subscribe();
 			_groups_subscribed = _groups_required;
-			/*
-			 * This event is needed to wake up the thread on CAN bus activity (RX/TX/Error).
-			 * Please note that with such multiplexing it is no longer possible to rely only on
-			 * the value returned from poll() to detect whether actuator control has timed out or not.
-			 * Instead, all ORB events need to be checked individually (see below).
-			 */
-			_poll_fds[_poll_fds_num] = ::pollfd();
-			_poll_fds[_poll_fds_num].fd = busevent_fd;
-			_poll_fds[_poll_fds_num].events = POLLIN;
-			_poll_fds_num += 1;
 		}
 
 		const int poll_ret = ::poll(_poll_fds, _poll_fds_num, PollTimeoutMs);
@@ -301,7 +302,7 @@ int UavcanNode::run()
 		} else {
 			// get controls for required topics
 			bool controls_updated = false;
-			unsigned poll_id = 0;
+			unsigned poll_id = 1;
 			for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					if (_poll_fds[poll_id].revents & POLLIN) {
@@ -317,7 +318,7 @@ int UavcanNode::run()
 				// XXX trigger failsafe
 			}
 
-			//can we mix?
+			// can we mix?
 			if (controls_updated && (_mixers != nullptr)) {
 
 				// XXX one output group has 8 outputs max,
@@ -417,7 +418,8 @@ UavcanNode::subscribe()
 	// Subscribe/unsubscribe to required actuator control groups
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
-	_poll_fds_num = 0;
+	// the first fd used by CAN
+	_poll_fds_num = 1;
 	for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
@@ -523,8 +525,8 @@ UavcanNode::print_info()
 		warnx("not running, start first");
 	}
 
-	warnx("groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
-	warnx("mixer: %s", (_mixers == nullptr) ? "FAIL" : "OK");
+	warnx("actuators control groups: sub: %u / req: %u / fds: %u", (unsigned)_groups_subscribed, (unsigned)_groups_required, _poll_fds_num);
+	warnx("mixer: %s", (_mixers == nullptr) ? "NONE" : "OK");
 }
 
 /*

From 020ac409be62e6c459e7aa5cb3181941c9bbfd23 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 3 Aug 2014 13:17:18 +0200
Subject: [PATCH 14/36] Remove airspeed and altitude raw data from TECS log, as
 we have these quantities already in the system log

---
 src/modules/fw_pos_control_l1/mtecs/mTecs.cpp |  6 ++----
 src/modules/sdlog2/sdlog2.c                   |  6 ++----
 src/modules/sdlog2/sdlog2_messages.h          |  4 +---
 src/modules/uORB/topics/tecs_status.h         | 12 ++++++------
 4 files changed, 11 insertions(+), 17 deletions(-)

diff --git a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
index 749f57a2ba..bffeefc1ff 100644
--- a/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
+++ b/src/modules/fw_pos_control_l1/mtecs/mTecs.cpp
@@ -109,8 +109,7 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 
 	/* Write part of the status message */
 	_status.altitudeSp = altitudeSp;
-	_status.altitude = altitude;
-	_status.altitudeFiltered = altitudeFiltered;
+	_status.altitude_filtered = altitudeFiltered;
 
 
 	/* use flightpath angle setpoint for total energy control */
@@ -146,8 +145,7 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 
 	/* Write part of the status message */
 	_status.airspeedSp = airspeedSp;
-	_status.airspeed = airspeed;
-	_status.airspeedFiltered = airspeedFiltered;
+	_status.airspeed_filtered = airspeedFiltered;
 
 
 	/* use longitudinal acceleration setpoint for total energy control */
diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c
index 31861c3fc8..6c4b494522 100644
--- a/src/modules/sdlog2/sdlog2.c
+++ b/src/modules/sdlog2/sdlog2.c
@@ -1597,14 +1597,12 @@ int sdlog2_thread_main(int argc, char *argv[])
 		if (copy_if_updated(ORB_ID(tecs_status), subs.tecs_status_sub, &buf.tecs_status)) {
 			log_msg.msg_type = LOG_TECS_MSG;
 			log_msg.body.log_TECS.altitudeSp = buf.tecs_status.altitudeSp;
-			log_msg.body.log_TECS.altitude = buf.tecs_status.altitude;
-			log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitudeFiltered;
+			log_msg.body.log_TECS.altitudeFiltered = buf.tecs_status.altitude_filtered;
 			log_msg.body.log_TECS.flightPathAngleSp = buf.tecs_status.flightPathAngleSp;
 			log_msg.body.log_TECS.flightPathAngle = buf.tecs_status.flightPathAngle;
 			log_msg.body.log_TECS.flightPathAngleFiltered = buf.tecs_status.flightPathAngleFiltered;
 			log_msg.body.log_TECS.airspeedSp = buf.tecs_status.airspeedSp;
-			log_msg.body.log_TECS.airspeed = buf.tecs_status.airspeed;
-			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeedFiltered;
+			log_msg.body.log_TECS.airspeedFiltered = buf.tecs_status.airspeed_filtered;
 			log_msg.body.log_TECS.airspeedDerivativeSp = buf.tecs_status.airspeedDerivativeSp;
 			log_msg.body.log_TECS.airspeedDerivative = buf.tecs_status.airspeedDerivative;
 			log_msg.body.log_TECS.totalEnergyRateSp = buf.tecs_status.totalEnergyRateSp;
diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h
index 853a3811f6..fb7609435b 100644
--- a/src/modules/sdlog2/sdlog2_messages.h
+++ b/src/modules/sdlog2/sdlog2_messages.h
@@ -333,13 +333,11 @@ struct log_GS1B_s {
 #define LOG_TECS_MSG 30
 struct log_TECS_s {
 	float altitudeSp;
-	float altitude;
 	float altitudeFiltered;
 	float flightPathAngleSp;
 	float flightPathAngle;
 	float flightPathAngleFiltered;
 	float airspeedSp;
-	float airspeed;
 	float airspeedFiltered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
@@ -455,7 +453,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(GS0B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1A, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
 	LOG_FORMAT(GS1B, "BBBBBBBBBBBBBBBB",	"s0,s1,s2,s3,s4,s5,s6,s7,s8,s9,s10,s11,s12,s13,s14,s15"),
-	LOG_FORMAT(TECS, "fffffffffffffffB",	"ASP,A,AF,FSP,F,FF,AsSP,As,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
+	LOG_FORMAT(TECS, "fffffffffffffB",	"ASP,AF,FSP,F,FF,AsSP,AsF,AsDSP,AsD,TERSP,TER,EDRSP,EDR,M"),
 	LOG_FORMAT(WIND, "ffff",	"X,Y,CovX,CovY"),
 
 	/* system-level messages, ID >= 0x80 */
diff --git a/src/modules/uORB/topics/tecs_status.h b/src/modules/uORB/topics/tecs_status.h
index 33055018ce..ddca19d61e 100644
--- a/src/modules/uORB/topics/tecs_status.h
+++ b/src/modules/uORB/topics/tecs_status.h
@@ -51,11 +51,13 @@
  */
 
 typedef enum {
-	TECS_MODE_NORMAL,
+	TECS_MODE_NORMAL = 0,
 	TECS_MODE_UNDERSPEED,
 	TECS_MODE_TAKEOFF,
 	TECS_MODE_LAND,
-	TECS_MODE_LAND_THROTTLELIM
+	TECS_MODE_LAND_THROTTLELIM,
+	TECS_MODE_BAD_DESCENT,
+	TECS_MODE_CLIMBOUT
 } tecs_mode;
 
  /**
@@ -65,14 +67,12 @@ struct tecs_status_s {
 	uint64_t timestamp;		/**< timestamp, in microseconds since system start */
 
 	float altitudeSp;
-	float altitude;
-	float altitudeFiltered;
+	float altitude_filtered;
 	float flightPathAngleSp;
 	float flightPathAngle;
 	float flightPathAngleFiltered;
 	float airspeedSp;
-	float airspeed;
-	float airspeedFiltered;
+	float airspeed_filtered;
 	float airspeedDerivativeSp;
 	float airspeedDerivative;
 

From 43ef622725b1114beccb722bad0538c4c0cc3175 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 3 Aug 2014 13:18:22 +0200
Subject: [PATCH 15/36] Reinstate TECS logging

---
 src/lib/external_lgpl/tecs/tecs.cpp | 40 +++++++++++++++---------
 src/lib/external_lgpl/tecs/tecs.h   | 47 ++++++++++++++++++-----------
 2 files changed, 56 insertions(+), 31 deletions(-)

diff --git a/src/lib/external_lgpl/tecs/tecs.cpp b/src/lib/external_lgpl/tecs/tecs.cpp
index 579fb5d42c..a57a0481a6 100644
--- a/src/lib/external_lgpl/tecs/tecs.cpp
+++ b/src/lib/external_lgpl/tecs/tecs.cpp
@@ -554,18 +554,30 @@ void TECS::update_pitch_throttle(const math::Matrix<3,3> &rotMat, float pitch, f
 	// Calculate pitch demand
 	_update_pitch();
 
-//    // Write internal variables to the log_tuning structure. This
-//    // structure will be logged in dataflash at 10Hz
-	// log_tuning.hgt_dem  = _hgt_dem_adj;
-	// log_tuning.hgt      = _integ3_state;
-	// log_tuning.dhgt_dem = _hgt_rate_dem;
-	// log_tuning.dhgt     = _integ2_state;
-	// log_tuning.spd_dem  = _TAS_dem_adj;
-	// log_tuning.spd      = _integ5_state;
-	// log_tuning.dspd     = _vel_dot;
-	// log_tuning.ithr     = _integ6_state;
-	// log_tuning.iptch    = _integ7_state;
-	// log_tuning.thr      = _throttle_dem;
-	// log_tuning.ptch     = _pitch_dem;
-	// log_tuning.dspd_dem = _TAS_rate_dem;
+	_tecs_state.timestamp = now;
+
+	if (_underspeed) {
+		_tecs_state.mode = ECL_TECS_MODE_UNDERSPEED;
+	} else if (_badDescent) {
+		_tecs_state.mode = ECL_TECS_MODE_BAD_DESCENT;
+	} else if (_climbOutDem) {
+		_tecs_state.mode = ECL_TECS_MODE_CLIMBOUT;
+	} else {
+		// If no error flag applies, conclude normal
+		_tecs_state.mode = ECL_TECS_MODE_NORMAL;
+	}
+
+	_tecs_state.hgt_dem  = _hgt_dem_adj;
+	_tecs_state.hgt      = _integ3_state;
+	_tecs_state.dhgt_dem = _hgt_rate_dem;
+	_tecs_state.dhgt     = _integ2_state;
+	_tecs_state.spd_dem  = _TAS_dem_adj;
+	_tecs_state.spd      = _integ5_state;
+	_tecs_state.dspd     = _vel_dot;
+	_tecs_state.ithr     = _integ6_state;
+	_tecs_state.iptch    = _integ7_state;
+	_tecs_state.thr      = _throttle_dem;
+	_tecs_state.ptch     = _pitch_dem;
+	_tecs_state.dspd_dem = _TAS_rate_dem;
+
 }
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 752b0ddd9c..bcc2d90e5c 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/src/lib/external_lgpl/tecs/tecs.h
@@ -28,6 +28,7 @@ class __EXPORT TECS
 {
 public:
 	TECS() :
+		_tecs_state {},
 		_update_50hz_last_usec(0),
 		_update_speed_last_usec(0),
 		_update_pitch_throttle_last_usec(0),
@@ -120,24 +121,33 @@ public:
 		return _spdWeight;
 	}
 
-	// log data on internal state of the controller. Called at 10Hz
-	// void log_data(DataFlash_Class &dataflash, uint8_t msgid);
+	enum ECL_TECS_MODE {
+		ECL_TECS_MODE_NORMAL = 0,
+		ECL_TECS_MODE_UNDERSPEED,
+		ECL_TECS_MODE_BAD_DESCENT,
+		ECL_TECS_MODE_CLIMBOUT
+	};
 
-	// struct PACKED log_TECS_Tuning {
-	// 	LOG_PACKET_HEADER;
-	// 	float hgt;
-	// 	float dhgt;
-	// 	float hgt_dem;
-	// 	float dhgt_dem;
-	// 	float spd_dem;
-	// 	float spd;
-	// 	float dspd;
-	// 	float ithr;
-	// 	float iptch;
-	// 	float thr;
-	// 	float ptch;
-	// 	float dspd_dem;
-	// } log_tuning;
+	struct tecs_state {
+		uint64_t timestamp;
+		float hgt;
+		float dhgt;
+		float hgt_dem;
+		float dhgt_dem;
+		float spd_dem;
+		float spd;
+		float dspd;
+		float ithr;
+		float iptch;
+		float thr;
+		float ptch;
+		float dspd_dem;
+		enum ECL_TECS_MODE mode;
+	};
+
+	void get_tecs_state(struct tecs_state& state) {
+		state = _tecs_state;
+	}
 
 	void set_time_const(float time_const) {
 		_timeConst = time_const;
@@ -212,6 +222,9 @@ public:
 	}
 
 private:
+
+	struct tecs_state _tecs_state;
+
 	// Last time update_50Hz was called
 	uint64_t _update_50hz_last_usec;
 

From ff760d07b4d115ce0bc58ca8e8f98a915c4bbb67 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 3 Aug 2014 13:19:10 +0200
Subject: [PATCH 16/36] Add TECS logging

---
 .../fw_pos_control_l1_main.cpp                | 47 +++++++++++++++++++
 1 file changed, 47 insertions(+)

diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 189a19d48f..350ce6dec1 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -146,6 +146,7 @@ private:
 	int		_range_finder_sub;		/**< range finder subscription */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
+	orb_advert_t	_tecs_status_pub;		/**< TECS status publication */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
 
 	struct vehicle_attitude_s			_att;				/**< vehicle attitude */
@@ -414,6 +415,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 /* publications */
 	_attitude_sp_pub(-1),
+	_tecs_status_pub(-1),
 	_nav_capabilities_pub(-1),
 
 /* states */
@@ -1384,6 +1386,51 @@ void FixedwingPositionControl::tecs_update_pitch_throttle(float alt_sp, float v_
 					    climbout_mode, climbout_pitch_min_rad,
 					    throttle_min, throttle_max, throttle_cruise,
 					    pitch_min_rad, pitch_max_rad);
+
+		struct TECS::tecs_state s;
+		_tecs.get_tecs_state(s);
+
+		struct tecs_status_s t;
+
+		t.timestamp = s.timestamp;
+
+		switch (s.mode) {
+			case TECS::ECL_TECS_MODE_NORMAL:
+				t.mode = TECS_MODE_NORMAL;
+				break;
+			case TECS::ECL_TECS_MODE_UNDERSPEED:
+				t.mode = TECS_MODE_UNDERSPEED;
+				break;
+			case TECS::ECL_TECS_MODE_BAD_DESCENT:
+				t.mode = TECS_MODE_BAD_DESCENT;
+				break;
+			case TECS::ECL_TECS_MODE_CLIMBOUT:
+				t.mode = TECS_MODE_CLIMBOUT;
+				break;
+		}
+
+		t.altitudeSp			= s.hgt_dem;
+		t.altitude_filtered		= s.hgt;
+		t.airspeedSp			= s.spd_dem;
+		t.airspeed_filtered		= s.spd;
+
+		t.flightPathAngleSp		= s.dhgt_dem;
+		t.flightPathAngle		= s.dhgt;
+		t.flightPathAngleFiltered	= s.dhgt;
+
+		t.airspeedDerivativeSp		= s.dspd_dem;
+		t.airspeedDerivative		= s.dspd;
+
+		t.totalEnergyRateSp		= s.thr;
+		t.totalEnergyRate		= s.ithr;
+		t.energyDistributionRateSp	= s.ptch;
+		t.energyDistributionRate	= s.iptch;
+
+		if (_tecs_status_pub > 0) {
+			orb_publish(ORB_ID(tecs_status), _tecs_status_pub, &t);
+		} else {
+			_tecs_status_pub = orb_advertise(ORB_ID(tecs_status), &t);
+		}
 	}
 }
 

From 6fbeaeaacecfa532c1c4437ba51312e1c46d980d Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Mon, 4 Aug 2014 09:40:38 -0700
Subject: [PATCH 17/36] Update arming_state_transition calls

Add new fRunPreArmChecks flag
---
 src/modules/commander/commander.cpp | 18 +++++++++---------
 1 file changed, 9 insertions(+), 9 deletions(-)

diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 2b528eef66..4f976546e1 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -393,7 +393,7 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 
 	// 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_local);
+	arming_res = arming_state_transition(&status, &safety, arm ? ARMING_STATE_ARMED : ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd_local);
 
 	if (arming_res == TRANSITION_CHANGED && mavlink_fd) {
 		mavlink_log_info(mavlink_fd_local, "[cmd] %s by %s", arm ? "ARMED" : "DISARMED", armedBy);
@@ -1085,7 +1085,7 @@ int commander_thread_main(int argc, char *argv[])
 			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_fd)) {
+				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
 					mavlink_log_info(mavlink_fd, "DISARMED by safety switch");
 					arming_state_changed = true;
 				}
@@ -1288,14 +1288,14 @@ int commander_thread_main(int argc, char *argv[])
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
 
 			if (armed.armed) {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, mavlink_fd);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
 					arming_state_changed = true;
 				}
 
 			} else {
-				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, mavlink_fd);
+				arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY_ERROR, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				if (arming_ret == TRANSITION_CHANGED) {
 					arming_state_changed = true;
@@ -1309,7 +1309,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* If in INIT state, try to proceed to STANDBY state */
 		if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) {
 			/* TODO: check for sensors */
-			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+			arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 			if (arming_ret == TRANSITION_CHANGED) {
 				arming_state_changed = true;
@@ -1368,7 +1368,7 @@ int commander_thread_main(int argc, char *argv[])
 				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);
-					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, mavlink_fd);
+					arming_ret = arming_state_transition(&status, &safety, new_arming_state, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 					if (arming_ret == TRANSITION_CHANGED) {
 						arming_state_changed = true;
 					}
@@ -1394,7 +1394,7 @@ int commander_thread_main(int argc, char *argv[])
 					if (status.main_state != MAIN_STATE_MANUAL) {
 						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 					} else {
-						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, mavlink_fd);
+						arming_ret = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 						if (arming_ret == TRANSITION_CHANGED) {
 							arming_state_changed = true;
 						}
@@ -2156,7 +2156,7 @@ void *commander_low_prio_loop(void *arg)
 				int calib_ret = ERROR;
 
 				/* try to go to INIT/PREFLIGHT arming state */
-				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, mavlink_fd)) {
+				if (TRANSITION_DENIED == arming_state_transition(&status, &safety, ARMING_STATE_INIT, &armed, true /* fRunPreArmChecks */, mavlink_fd)) {
 					answer_command(cmd, VEHICLE_CMD_RESULT_DENIED);
 					break;
 				}
@@ -2219,7 +2219,7 @@ void *commander_low_prio_loop(void *arg)
 					tune_negative(true);
 				}
 
-				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, mavlink_fd);
+				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed, true /* fRunPreArmChecks */, mavlink_fd);
 
 				break;
 			}

From 014fd5f47baad2f49292216eba851100aa543fef Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Mon, 4 Aug 2014 09:41:29 -0700
Subject: [PATCH 18/36] Add fRunPreArmChecks flag

This is to allow unit tests to be written which do not perform pre-arm
checks
---
 src/modules/commander/state_machine_helper.cpp | 13 +++++++------
 src/modules/commander/state_machine_helper.h   |  2 +-
 2 files changed, 8 insertions(+), 7 deletions(-)

diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp
index 3c3d2f2335..f8589d24b8 100644
--- a/src/modules/commander/state_machine_helper.cpp
+++ b/src/modules/commander/state_machine_helper.cpp
@@ -99,11 +99,12 @@ static const char * const state_names[ARMING_STATE_MAX] = {
 };
 
 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
+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
+			bool			fRunPreArmChecks,	///< true: run the pre-arm checks, false: no pre-arm checks, for unit testing
+			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);
@@ -125,7 +126,7 @@ arming_state_transition(struct vehicle_status_s *status,            /// current
 		int prearm_ret = OK;
 
 		/* only perform the check if we have to */
-		if (new_arming_state == ARMING_STATE_ARMED) {
+		if (fRunPreArmChecks && new_arming_state == ARMING_STATE_ARMED) {
 			prearm_ret = prearm_check(status, mavlink_fd);
 		}
 
diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h
index bb1b87e712..69ce8bbcee 100644
--- a/src/modules/commander/state_machine_helper.h
+++ b/src/modules/commander/state_machine_helper.h
@@ -57,7 +57,7 @@ typedef enum {
 bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed);
 
 transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety,
-		arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd);
+		arming_state_t new_arming_state, struct actuator_armed_s *armed, bool fRunPreArmChecks, const int mavlink_fd);
 
 transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state);
 

From cfe5c45e04f65f24110f222c3de02147146081e1 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Mon, 4 Aug 2014 09:42:16 -0700
Subject: [PATCH 19/36] Fix broken arming unit test

Also, update arming_state_transition calls to add new fRunPreArmChecks
flag.
---
 .../commander_tests/state_machine_helper_test.cpp      | 10 +++++-----
 1 file changed, 5 insertions(+), 5 deletions(-)

diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
index 2e18c42841..08dda2fabf 100644
--- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp
+++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp
@@ -286,7 +286,7 @@ bool StateMachineHelperTest::armingStateTransitionTest(void)
         armed.ready_to_arm = test->current_state.ready_to_arm;
         
         // Attempt transition
-        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, 0 /* no mavlink_fd */);
+        transition_result_t result = arming_state_transition(&status, &safety, test->requested_state, &armed, false /* no pre-arm checks */, 0 /* no mavlink_fd */);
         
         // Validate result of transition
         ut_assert(test->assertMsg, test->expected_transition_result == result);
@@ -335,12 +335,12 @@ bool StateMachineHelperTest::mainStateTransitionTest(void)
 			MTT_ALL_NOT_VALID,
 			MAIN_STATE_ACRO, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
-		{ "transition: MANUAL to AUTO_MISSION - global position valid",
-			MTT_GLOBAL_POS_VALID,
+		{ "transition: MANUAL to AUTO_MISSION - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
 			MAIN_STATE_MANUAL, MAIN_STATE_AUTO_MISSION, TRANSITION_CHANGED },
 
-		{ "transition: AUTO_MISSION to MANUAL - global position valid",
-			MTT_GLOBAL_POS_VALID,
+		{ "transition: AUTO_MISSION to MANUAL - global position valid, home position valid",
+			MTT_GLOBAL_POS_VALID | MTT_HOME_POS_VALID,
 			MAIN_STATE_AUTO_MISSION, MAIN_STATE_MANUAL, TRANSITION_CHANGED },
 
 		{ "transition: MANUAL to AUTO_LOITER - global position valid",

From 33762ce861b06cfa521d75fc18df3c7237e3a423 Mon Sep 17 00:00:00 2001
From: Pavel Kirienko <pavel.kirienko@gmail.com>
Date: Wed, 6 Aug 2014 12:40:07 +0400
Subject: [PATCH 20/36] UAVCAN ESC mixer: removed the failsafe placeholder,
 it's no use here

---
 src/modules/uavcan/uavcan_main.cpp | 5 -----
 1 file changed, 5 deletions(-)

diff --git a/src/modules/uavcan/uavcan_main.cpp b/src/modules/uavcan/uavcan_main.cpp
index 5b1539e3ed..4535b6d6ad 100644
--- a/src/modules/uavcan/uavcan_main.cpp
+++ b/src/modules/uavcan/uavcan_main.cpp
@@ -313,11 +313,6 @@ int UavcanNode::run()
 				}
 			}
 
-			if (!controls_updated) {
-				// timeout: no control data, switch to failsafe values
-				// XXX trigger failsafe
-			}
-
 			// can we mix?
 			if (controls_updated && (_mixers != nullptr)) {
 

From 13b9cd0cec2d10d6252d0e8015d000f4fcc1517f Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 7 Aug 2014 07:25:20 +0200
Subject: [PATCH 21/36] Do not fuse height after innovation consistency check
 fail

---
 .../ekf_att_pos_estimator/estimator_23states.cpp       | 10 +++++++++-
 1 file changed, 9 insertions(+), 1 deletion(-)

diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index ffdd29a5b7..c619735c61 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -1070,10 +1070,18 @@ void AttPosEKF::FuseVelposNED()
             // apply a 10-sigma threshold
             current_ekf_state.hgtHealth = sq(hgtInnov) < 100.0f*varInnovVelPos[5];
             current_ekf_state.hgtTimeout = (millis() - current_ekf_state.hgtFailTime) > hgtRetryTime;
-            if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout)
+            if (current_ekf_state.hgtHealth || current_ekf_state.hgtTimeout || staticMode)
             {
                 current_ekf_state.hgtHealth = true;
                 current_ekf_state.hgtFailTime = millis();
+
+                // if we just reset from a timeout, do not fuse
+                // the height data, but reset height and stored states
+                if (current_ekf_state.hgtTimeout) {
+                    ResetHeight();
+                    ResetStoredStates();
+                    fuseHgtData = false;
+                }
             }
             else
             {

From 20c3a329c72ca5950f7bb037473a812b4dd74b1b Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Thu, 7 Aug 2014 07:36:59 +0200
Subject: [PATCH 22/36] Introduce similar checks fo all other health checks in
 the filter

---
 .../estimator_23states.cpp                    | 21 +++++++++++++++++--
 1 file changed, 19 insertions(+), 2 deletions(-)

diff --git a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
index c619735c61..c7c7305b2a 100644
--- a/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
+++ b/src/modules/ekf_att_pos_estimator/estimator_23states.cpp
@@ -1032,10 +1032,16 @@ void AttPosEKF::FuseVelposNED()
             // apply a 5-sigma threshold
             current_ekf_state.velHealth = (sq(velInnov[0]) + sq(velInnov[1]) + sq(velInnov[2])) < 25.0f * (varInnovVelPos[0] + varInnovVelPos[1] + varInnovVelPos[2]);
             current_ekf_state.velTimeout = (millis() - current_ekf_state.velFailTime) > horizRetryTime;
-            if (current_ekf_state.velHealth || current_ekf_state.velTimeout)
-            {
+            if (current_ekf_state.velHealth || staticMode) {
                 current_ekf_state.velHealth = true;
                 current_ekf_state.velFailTime = millis();
+            } else if (current_ekf_state.velTimeout || !current_ekf_state.posHealth) {
+                // XXX check
+                current_ekf_state.velHealth = true;
+                ResetVelocity();
+                ResetStoredStates();
+                // do not fuse bad data
+                fuseVelData = false;
             }
             else
             {
@@ -1056,6 +1062,17 @@ void AttPosEKF::FuseVelposNED()
             {
                 current_ekf_state.posHealth = true;
                 current_ekf_state.posFailTime = millis();
+
+                if (current_ekf_state.posTimeout) {
+                    ResetPosition();
+                    
+                    // XXX cross-check the state reset
+                    ResetStoredStates();
+
+                    // do not fuse position data on this time
+                    // step
+                    fusePosData = false;
+                }
             }
             else
             {

From faaeaeb11333598abd17db2189a2bc47216d0a98 Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 8 Aug 2014 00:22:57 +0200
Subject: [PATCH 23/36] mc_pos_control: manual and offboard control
 reorganization and cleanup

---
 .../mc_pos_control/mc_pos_control_main.cpp    | 271 +++++++++---------
 .../uORB/topics/position_setpoint_triplet.h   |   8 +-
 2 files changed, 140 insertions(+), 139 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index d22f43b5af..a0837a2ddc 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -105,6 +105,7 @@ public:
 	int		start();
 
 private:
+	const float alt_ctl_dz = 0.2f;
 
 	bool		_task_should_exit;		/**< if true, task should exit */
 	int		_control_task;			/**< task handle for task */
@@ -184,6 +185,8 @@ private:
 	math::Vector<3> _vel;
 	math::Vector<3> _vel_sp;
 	math::Vector<3> _vel_prev;			/**< velocity on previous step */
+	math::Vector<3> _vel_ff;
+	math::Vector<3> _sp_move_rate;
 
 	/**
 	 * Update our local parameter cache.
@@ -216,6 +219,16 @@ private:
 	 */
 	void		reset_alt_sp();
 
+	/**
+	 * Set position setpoint using manual control
+	 */
+	void		control_manual(float dt);
+
+	/**
+	 * Set position setpoint using offboard control
+	 */
+	void		control_offboard(float dt);
+
 	/**
 	 * Select between barometric and global (AMSL) altitudes
 	 */
@@ -297,6 +310,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_vel.zero();
 	_vel_sp.zero();
 	_vel_prev.zero();
+	_vel_ff.zero();
+	_sp_move_rate.zero();
 
 	_params_handles.thr_min		= param_find("MPC_THR_MIN");
 	_params_handles.thr_max		= param_find("MPC_THR_MAX");
@@ -515,6 +530,120 @@ MulticopterPositionControl::reset_alt_sp()
 	}
 }
 
+void
+MulticopterPositionControl::control_manual(float dt)
+{
+	_sp_move_rate.zero();
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		/* move altitude setpoint with throttle stick */
+		_sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
+	}
+
+	if (_control_mode.flag_control_position_enabled) {
+		/* move position setpoint with roll/pitch stick */
+		_sp_move_rate(0) = _manual.x;
+		_sp_move_rate(1) = _manual.y;
+	}
+
+	/* limit setpoint move rate */
+	float sp_move_norm = _sp_move_rate.length();
+
+	if (sp_move_norm > 1.0f) {
+		_sp_move_rate /= sp_move_norm;
+	}
+
+	/* _sp_move_rate scaled to 0..1, scale it to max speed and rotate around yaw */
+	math::Matrix<3, 3> R_yaw_sp;
+	R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
+	_sp_move_rate = R_yaw_sp * _sp_move_rate.emult(_params.vel_max);
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		/* reset alt setpoint to current altitude if needed */
+		reset_alt_sp();
+	}
+
+	if (_control_mode.flag_control_position_enabled) {
+		/* reset position setpoint to current position if needed */
+		reset_pos_sp();
+	}
+
+	/* feed forward setpoint move rate with weight vel_ff */
+	_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+	/* move position setpoint */
+	_pos_sp += _sp_move_rate * dt;
+
+	/* check if position setpoint is too far from actual position */
+	math::Vector<3> pos_sp_offs;
+	pos_sp_offs.zero();
+
+	if (_control_mode.flag_control_position_enabled) {
+		pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
+		pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
+	}
+
+	if (_control_mode.flag_control_altitude_enabled) {
+		pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
+	}
+
+	float pos_sp_offs_norm = pos_sp_offs.length();
+
+	if (pos_sp_offs_norm > 1.0f) {
+		pos_sp_offs /= pos_sp_offs_norm;
+		_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
+	}
+}
+
+void
+MulticopterPositionControl::control_offboard(float dt)
+{
+	bool updated;
+	orb_check(_pos_sp_triplet_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
+	}
+
+	if (_pos_sp_triplet.current.valid) {
+		if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
+			/* control position */
+			_pos_sp(0) = _pos_sp_triplet.current.x;
+			_pos_sp(1) = _pos_sp_triplet.current.y;
+			_pos_sp(2) = _pos_sp_triplet.current.z;
+			_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
+
+		} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
+			/* control velocity */
+			/* reset position setpoint to current position if needed */
+			reset_pos_sp();
+
+			/* set position setpoint move rate */
+			_sp_move_rate(0) = _pos_sp_triplet.current.vx;
+			_sp_move_rate(1) = _pos_sp_triplet.current.vy;
+			_att_sp.yaw_body = _att_sp.yaw_body + _pos_sp_triplet.current.yawspeed * dt;
+		}
+
+		if (_control_mode.flag_control_altitude_enabled) {
+			/* reset alt setpoint to current altitude if needed */
+			reset_alt_sp();
+
+			/* set altitude setpoint move rate */
+			_sp_move_rate(2) = _pos_sp_triplet.current.vz;
+		}
+
+		/* feed forward setpoint move rate with weight vel_ff */
+		_vel_ff = _sp_move_rate.emult(_params.vel_ff);
+
+		/* move position setpoint */
+		_pos_sp += _sp_move_rate * dt;
+
+	} else {
+		reset_pos_sp();
+		reset_alt_sp();
+	}
+}
+
 void
 MulticopterPositionControl::task_main()
 {
@@ -553,13 +682,6 @@ MulticopterPositionControl::task_main()
 
 	hrt_abstime t_prev = 0;
 
-	const float alt_ctl_dz = 0.2f;
-
-	math::Vector<3> sp_move_rate;
-	sp_move_rate.zero();
-
-	float yaw_sp_move_rate;
-
 	math::Vector<3> thrust_int;
 	thrust_int.zero();
 	math::Matrix<3, 3> R;
@@ -618,138 +740,17 @@ MulticopterPositionControl::task_main()
 			_vel(1) = _local_pos.vy;
 			_vel(2) = _local_pos.vz;
 
-			sp_move_rate.zero();
+			_vel_ff.zero();
+			_sp_move_rate.zero();
 
 			/* select control source */
 			if (_control_mode.flag_control_manual_enabled) {
 				/* manual control */
-				if (_control_mode.flag_control_altitude_enabled) {
-					/* reset alt setpoint to current altitude if needed */
-					reset_alt_sp();
-
-					/* move altitude setpoint with throttle stick */
-					sp_move_rate(2) = -scale_control(_manual.z - 0.5f, 0.5f, alt_ctl_dz);
-				}
-
-				if (_control_mode.flag_control_position_enabled) {
-					/* reset position setpoint to current position if needed */
-					reset_pos_sp();
-
-					/* move position setpoint with roll/pitch stick */
-					sp_move_rate(0) = _manual.x;
-					sp_move_rate(1) = _manual.y;
-				}
-
-				/* limit setpoint move rate */
-				float sp_move_norm = sp_move_rate.length();
-
-				if (sp_move_norm > 1.0f) {
-					sp_move_rate /= sp_move_norm;
-				}
-
-				/* scale to max speed and rotate around yaw */
-				math::Matrix<3, 3> R_yaw_sp;
-				R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
-				sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
-
-				/* move position setpoint */
-				_pos_sp += sp_move_rate * dt;
-
-				/* check if position setpoint is too far from actual position */
-				math::Vector<3> pos_sp_offs;
-				pos_sp_offs.zero();
-
-				if (_control_mode.flag_control_position_enabled) {
-					pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
-					pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
-				}
-
-				if (_control_mode.flag_control_altitude_enabled) {
-					pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
-				}
-
-				float pos_sp_offs_norm = pos_sp_offs.length();
-
-				if (pos_sp_offs_norm > 1.0f) {
-					pos_sp_offs /= pos_sp_offs_norm;
-					_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
-				}
+				control_manual(dt);
 
 			} else if (_control_mode.flag_control_offboard_enabled) {
-				/* Offboard control */
-				bool updated;
-				orb_check(_pos_sp_triplet_sub, &updated);
-
-				if (updated) {
-					orb_copy(ORB_ID(position_setpoint_triplet), _pos_sp_triplet_sub, &_pos_sp_triplet);
-				}
-
-				if (_pos_sp_triplet.current.valid) {
-
-					if (_control_mode.flag_control_position_enabled && _pos_sp_triplet.current.position_valid) {
-
-						_pos_sp(0) = _pos_sp_triplet.current.x;
-						_pos_sp(1) = _pos_sp_triplet.current.y;
-						_pos_sp(2) = _pos_sp_triplet.current.z;
-						_att_sp.yaw_body = _pos_sp_triplet.current.yaw;
-
-					} else if (_control_mode.flag_control_velocity_enabled && _pos_sp_triplet.current.velocity_valid) {
-						/* reset position setpoint to current position if needed */
-						reset_pos_sp();
-						/* move position setpoint with roll/pitch stick */
-						sp_move_rate(0) = _pos_sp_triplet.current.vx;
-						sp_move_rate(1) = _pos_sp_triplet.current.vy;
-						yaw_sp_move_rate = _pos_sp_triplet.current.yawspeed;
-						_att_sp.yaw_body = _att.yaw + yaw_sp_move_rate * dt;
-					}
-
-					if (_control_mode.flag_control_altitude_enabled) {
-						/* reset alt setpoint to current altitude if needed */
-						reset_alt_sp();
-
-						/* move altitude setpoint with throttle stick */
-						sp_move_rate(2) = -scale_control(_pos_sp_triplet.current.vz - 0.5f, 0.5f, alt_ctl_dz);;
-					}
-
-					/* limit setpoint move rate */
-					float sp_move_norm = sp_move_rate.length();
-
-					if (sp_move_norm > 1.0f) {
-						sp_move_rate /= sp_move_norm;
-					}
-
-					/* scale to max speed and rotate around yaw */
-					math::Matrix<3, 3> R_yaw_sp;
-					R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
-					sp_move_rate = R_yaw_sp * sp_move_rate.emult(_params.vel_max);
-
-					/* move position setpoint */
-					_pos_sp += sp_move_rate * dt;
-
-					/* check if position setpoint is too far from actual position */
-					math::Vector<3> pos_sp_offs;
-					pos_sp_offs.zero();
-
-					if (_control_mode.flag_control_position_enabled) {
-						pos_sp_offs(0) = (_pos_sp(0) - _pos(0)) / _params.sp_offs_max(0);
-						pos_sp_offs(1) = (_pos_sp(1) - _pos(1)) / _params.sp_offs_max(1);
-					}
-
-					if (_control_mode.flag_control_altitude_enabled) {
-						pos_sp_offs(2) = (_pos_sp(2) - _pos(2)) / _params.sp_offs_max(2);
-					}
-
-					float pos_sp_offs_norm = pos_sp_offs.length();
-
-					if (pos_sp_offs_norm > 1.0f) {
-						pos_sp_offs /= pos_sp_offs_norm;
-						_pos_sp = _pos + pos_sp_offs.emult(_params.sp_offs_max);
-					}
-
-				} else {
-					reset_pos_sp();
-					reset_alt_sp();
-				}
+				/* offboard control */
+				control_offboard(dt);
 
 			} else {
 				/* AUTO */
@@ -823,7 +824,7 @@ MulticopterPositionControl::task_main()
 				/* run position & altitude controllers, calculate velocity setpoint */
 				math::Vector<3> pos_err = _pos_sp - _pos;
 
-				_vel_sp = pos_err.emult(_params.pos_p) + sp_move_rate.emult(_params.vel_ff);
+				_vel_sp = pos_err.emult(_params.pos_p) + _vel_ff;
 
 				if (!_control_mode.flag_control_altitude_enabled) {
 					_reset_alt_sp = true;
@@ -903,7 +904,7 @@ MulticopterPositionControl::task_main()
 					math::Vector<3> vel_err = _vel_sp - _vel;
 
 					/* derivative of velocity error, not includes setpoint acceleration */
-					math::Vector<3> vel_err_d = (sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
+					math::Vector<3> vel_err_d = (_sp_move_rate - _vel).emult(_params.pos_p) - (_vel - _vel_prev) / dt;
 					_vel_prev = _vel;
 
 					/* thrust vector in NED frame */
diff --git a/src/modules/uORB/topics/position_setpoint_triplet.h b/src/modules/uORB/topics/position_setpoint_triplet.h
index 4a19321809..ec2131abd8 100644
--- a/src/modules/uORB/topics/position_setpoint_triplet.h
+++ b/src/modules/uORB/topics/position_setpoint_triplet.h
@@ -60,7 +60,7 @@ enum SETPOINT_TYPE
 	SETPOINT_TYPE_TAKEOFF,		/**< takeoff setpoint */
 	SETPOINT_TYPE_LAND,		/**< land setpoint, altitude must be ignored, descend until landing */
 	SETPOINT_TYPE_IDLE,		/**< do nothing, switch off motors or keep at idle speed (MC) */
-	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint set by offboard */
+	SETPOINT_TYPE_OFFBOARD, 	/**< setpoint in NED frame (x, y, z, vx, vy, vz) set by offboard */
 };
 
 struct position_setpoint_s
@@ -71,9 +71,9 @@ struct position_setpoint_s
 	float y;			/**< local position setpoint in m in NED */
 	float z;			/**< local position setpoint in m in NED */
 	bool position_valid;	/**< true if local position setpoint valid */
-	float vx;			/**< local velocity setpoint in m in NED */
-	float vy;			/**< local velocity setpoint in m in NED */
-	float vz;			/**< local velocity setpoint in m in NED */
+	float vx;			/**< local velocity setpoint in m/s in NED */
+	float vy;			/**< local velocity setpoint in m/s in NED */
+	float vz;			/**< local velocity setpoint in m/s in NED */
 	bool velocity_valid;		/**< true if local velocity setpoint valid */
 	double lat;			/**< latitude, in deg */
 	double lon;			/**< longitude, in deg */

From 03f839a27ac61178be8ef77526faa13a46c4cd6b Mon Sep 17 00:00:00 2001
From: Anton Babushkin <anton.babushkin@me.com>
Date: Fri, 8 Aug 2014 00:24:18 +0200
Subject: [PATCH 24/36] mc_pos_control: more accurate position setpoint reset,
 keep attitude setpoint continuous

---
 src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 ++++++---
 1 file changed, 6 insertions(+), 3 deletions(-)

diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index a0837a2ddc..0106af80aa 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -514,8 +514,11 @@ MulticopterPositionControl::reset_pos_sp()
 {
 	if (_reset_pos_sp) {
 		_reset_pos_sp = false;
-		_pos_sp(0) = _pos(0) + _vel(0) / _params.pos_p(0) * (1.0f - _params.vel_ff(0));
-		_pos_sp(1) = _pos(1) + _vel(1) / _params.pos_p(1) * (1.0f - _params.vel_ff(1));
+		/* shift position setpoint to make attitude setpoint continuous */
+		_pos_sp(0) = _pos(0) + (_vel(0) - _att_sp.R_body[0][2] * _att_sp.thrust / _params.vel_p(0)
+				- _params.vel_ff(0) * _sp_move_rate(0)) / _params.pos_p(0);
+		_pos_sp(1) = _pos(1) + (_vel(1) - _att_sp.R_body[1][2] * _att_sp.thrust / _params.vel_p(1)
+				- _params.vel_ff(1) * _sp_move_rate(1)) / _params.pos_p(1);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset pos sp: %.2f, %.2f", (double)_pos_sp(0), (double)_pos_sp(1));
 	}
 }
@@ -525,7 +528,7 @@ MulticopterPositionControl::reset_alt_sp()
 {
 	if (_reset_alt_sp) {
 		_reset_alt_sp = false;
-		_pos_sp(2) = _pos(2) + _vel(2) / _params.pos_p(2) * (1.0f - _params.vel_ff(2));
+		_pos_sp(2) = _pos(2) + (_vel(2) - _params.vel_ff(2) * _sp_move_rate(2)) / _params.pos_p(2);
 		mavlink_log_info(_mavlink_fd, "[mpc] reset alt sp: %.2f", -(double)_pos_sp(2));
 	}
 }

From 5246d6b49211b7ba73a32c7b73967cabe5fd512a Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sat, 9 Aug 2014 19:31:34 +0200
Subject: [PATCH 25/36] Updated mavlink version

---
 mavlink/include/mavlink/v1.0 | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0
index 04b1ad5b28..96425caae3 160000
--- a/mavlink/include/mavlink/v1.0
+++ b/mavlink/include/mavlink/v1.0
@@ -1 +1 @@
-Subproject commit 04b1ad5b284d5e916858ca9f928e93d97bbf6ad9
+Subproject commit 96425caae3de0e8cda81405c1a0e9d2fb14369a2

From 312f9e0ccce9e3f530f6fc222e371a48c3b1af0d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 9 Aug 2014 19:43:57 +0200
Subject: [PATCH 26/36] mixers: adapted FX79 mixer to have more pitch influence

---
 ROMFS/px4fmu_common/mixers/FMU_FX79.mix | 4 ++--
 1 file changed, 2 insertions(+), 2 deletions(-)

diff --git a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix
index 0a1dca98d5..112d9b891c 100755
--- a/ROMFS/px4fmu_common/mixers/FMU_FX79.mix
+++ b/ROMFS/px4fmu_common/mixers/FMU_FX79.mix
@@ -27,12 +27,12 @@ for the elevons.
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0   5000   8000      0 -10000  10000
+S: 0 0   7500   7500      0 -10000  10000
 S: 0 1   8000   8000      0 -10000  10000
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0   8000   5000      0 -10000  10000
+S: 0 0   7500   7500      0 -10000  10000
 S: 0 1  -8000  -8000      0 -10000  10000
 
 Output 2

From 3fdf63d2b3cf25904371cf700e08ce4e8d214824 Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sun, 30 Mar 2014 15:00:37 +0200
Subject: [PATCH 27/36] Startup script: added viper script

Conflicts:
	mavlink/include/mavlink/v1.0
---
 ROMFS/px4fmu_common/init.d/3035_viper   | 10 ++++++++++
 ROMFS/px4fmu_common/init.d/rc.autostart |  5 +++++
 2 files changed, 15 insertions(+)
 create mode 100644 ROMFS/px4fmu_common/init.d/3035_viper

diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper
new file mode 100644
index 0000000000..9c8f8a585e
--- /dev/null
+++ b/ROMFS/px4fmu_common/init.d/3035_viper
@@ -0,0 +1,10 @@
+#!nsh
+#
+# Viper
+#
+# Simon Wilks <sjwilks@gmail.com>
+#
+
+sh /etc/init.d/rc.fw_defaults
+
+set MIXER FMU_FX79
\ No newline at end of file
diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart
index 17541e6800..9de7d9ecdb 100644
--- a/ROMFS/px4fmu_common/init.d/rc.autostart
+++ b/ROMFS/px4fmu_common/init.d/rc.autostart
@@ -103,6 +103,11 @@ then
 	sh /etc/init.d/3034_fx79
 fi
 
+if param compare SYS_AUTOSTART 3035 35
+then
+	sh /etc/init.d/3035_viper
+fi
+
 if param compare SYS_AUTOSTART 3100
 then
 	sh /etc/init.d/3100_tbs_caipirinha

From e6210058a3527bc87a74781159631a1f59ad053d Mon Sep 17 00:00:00 2001
From: Julian Oes <julian@oes.ch>
Date: Sat, 9 Aug 2014 19:52:30 +0200
Subject: [PATCH 28/36] mixers: added Viper mixer

---
 ROMFS/px4fmu_common/init.d/3035_viper |  2 +-
 ROMFS/px4fmu_common/mixers/Viper.mix  | 72 +++++++++++++++++++++++++++
 2 files changed, 73 insertions(+), 1 deletion(-)
 create mode 100755 ROMFS/px4fmu_common/mixers/Viper.mix

diff --git a/ROMFS/px4fmu_common/init.d/3035_viper b/ROMFS/px4fmu_common/init.d/3035_viper
index 9c8f8a585e..a4c1e832dc 100644
--- a/ROMFS/px4fmu_common/init.d/3035_viper
+++ b/ROMFS/px4fmu_common/init.d/3035_viper
@@ -7,4 +7,4 @@
 
 sh /etc/init.d/rc.fw_defaults
 
-set MIXER FMU_FX79
\ No newline at end of file
+set MIXER Viper
diff --git a/ROMFS/px4fmu_common/mixers/Viper.mix b/ROMFS/px4fmu_common/mixers/Viper.mix
new file mode 100755
index 0000000000..79c4447bee
--- /dev/null
+++ b/ROMFS/px4fmu_common/mixers/Viper.mix
@@ -0,0 +1,72 @@
+Viper Delta-wing mixer
+=================================
+
+Designed for Viper.
+
+TODO (sjwilks): Add mixers for flaps.
+
+This file defines mixers suitable for controlling a delta wing aircraft using
+PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
+servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
+assumed to be unused.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch) and 3 (thrust).
+
+See the README for more information on the scaler format.
+
+Elevon mixers
+-------------
+Three scalers total (output, roll, pitch).
+
+On the assumption that the two elevon servos are physically reversed, the pitch
+input is inverted between the two servos.
+
+The scaling factor for roll inputs is adjusted to implement differential travel
+for the elevons.
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0   7500   7500      0 -10000  10000
+S: 0 1   8000   8000      0 -10000  10000
+
+M: 2
+O:      10000  10000      0 -10000  10000
+S: 0 0   7500   7500      0 -10000  10000
+S: 0 1  -8000  -8000      0 -10000  10000
+
+Output 2
+--------
+This mixer is empty.
+
+Z:
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000
+
+Gimbal / flaps / payload mixer for last four channels
+-----------------------------------------------------
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 4  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 5  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 6  10000  10000      0 -10000  10000
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 7  10000  10000      0 -10000  10000

From 4b065a7e1d2a523ba40abbf63452bf89d1004cd8 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Aug 2014 00:52:03 +0200
Subject: [PATCH 29/36] Updated MAVLink version

---
 mavlink/include/mavlink/v1.0 | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0
index 96425caae3..8cc21472cc 160000
--- a/mavlink/include/mavlink/v1.0
+++ b/mavlink/include/mavlink/v1.0
@@ -1 +1 @@
-Subproject commit 96425caae3de0e8cda81405c1a0e9d2fb14369a2
+Subproject commit 8cc21472cce09709d4c651e2855d891843a17e41

From 5225d87854bdea1b5e4367e3db6b41ece9e46e13 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Aug 2014 01:24:04 +0200
Subject: [PATCH 30/36] Updated MAVLink to latest revision

---
 src/modules/mavlink/mavlink_messages.cpp | 155 ++++++++---------------
 src/modules/mavlink/mavlink_receiver.cpp |  34 -----
 2 files changed, 51 insertions(+), 138 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 683f0f1e84..7f67475f2e 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1385,43 +1385,43 @@ protected:
 };
 
 
-class MavlinkStreamGlobalPositionSetpointInt : public MavlinkStream
+class MavlinkStreamPositionTargetGlobalInt : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamGlobalPositionSetpointInt::get_name_static();
+		return MavlinkStreamPositionTargetGlobalInt::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "GLOBAL_POSITION_SETPOINT_INT";
+		return "POSITION_TARGET_GLOBAL_INT";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT;
+		return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamGlobalPositionSetpointInt(mavlink);
+		return new MavlinkStreamPositionTargetGlobalInt(mavlink);
 	}
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
 	MavlinkOrbSubscription *_pos_sp_triplet_sub;
 
 	/* do not allow top copying this class */
-	MavlinkStreamGlobalPositionSetpointInt(MavlinkStreamGlobalPositionSetpointInt &);
-	MavlinkStreamGlobalPositionSetpointInt& operator = (const MavlinkStreamGlobalPositionSetpointInt &);
+	MavlinkStreamPositionTargetGlobalInt(MavlinkStreamPositionTargetGlobalInt &);
+	MavlinkStreamPositionTargetGlobalInt& operator = (const MavlinkStreamPositionTargetGlobalInt &);
 
 protected:
-	explicit MavlinkStreamGlobalPositionSetpointInt(Mavlink *mavlink) : MavlinkStream(mavlink),
+	explicit MavlinkStreamPositionTargetGlobalInt(Mavlink *mavlink) : MavlinkStream(mavlink),
 		_pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet)))
 	{}
 
@@ -1430,15 +1430,14 @@ protected:
 		struct position_setpoint_triplet_s pos_sp_triplet;
 
 		if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
-			mavlink_global_position_setpoint_int_t msg;
+			mavlink_position_target_global_int_t msg;
 
 			msg.coordinate_frame = MAV_FRAME_GLOBAL;
-			msg.latitude = pos_sp_triplet.current.lat * 1e7;
-			msg.longitude = pos_sp_triplet.current.lon * 1e7;
-			msg.altitude = pos_sp_triplet.current.alt * 1000;
-			msg.yaw = pos_sp_triplet.current.yaw * M_RAD_TO_DEG_F * 100.0f;
+			msg.lat_int = pos_sp_triplet.current.lat * 1e7;
+			msg.lon_int = pos_sp_triplet.current.lon * 1e7;
+			msg.alt = pos_sp_triplet.current.alt;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_GLOBAL_POSITION_SETPOINT_INT, &msg);
+			_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_GLOBAL_INT, &msg);
 		}
 	}
 };
@@ -1454,12 +1453,12 @@ public:
 
 	static const char *get_name_static()
 	{
-		return "LOCAL_POSITION_SETPOINT";
+		return "POSITION_TARGET_LOCAL_NED";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT;
+		return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
@@ -1469,7 +1468,7 @@ public:
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
@@ -1491,137 +1490,86 @@ protected:
 		struct vehicle_local_position_setpoint_s pos_sp;
 
 		if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) {
-			mavlink_local_position_setpoint_t msg;
+			mavlink_position_target_local_ned_t msg;
 
 			msg.coordinate_frame = MAV_FRAME_LOCAL_NED;
 			msg.x = pos_sp.x;
 			msg.y = pos_sp.y;
 			msg.z = pos_sp.z;
-			msg.yaw = pos_sp.yaw;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_LOCAL_POSITION_SETPOINT, &msg);
+			_mavlink->send_message(MAVLINK_MSG_ID_POSITION_TARGET_LOCAL_NED, &msg);
 		}
 	}
 };
 
 
-class MavlinkStreamRollPitchYawThrustSetpoint : public MavlinkStream
+class MavlinkStreamAttitudeTarget : public MavlinkStream
 {
 public:
 	const char *get_name() const
 	{
-		return MavlinkStreamRollPitchYawThrustSetpoint::get_name_static();
+		return MavlinkStreamAttitudeTarget::get_name_static();
 	}
 
 	static const char *get_name_static()
 	{
-		return "ROLL_PITCH_YAW_THRUST_SETPOINT";
+		return "ATTITUDE_TARGET";
 	}
 
 	uint8_t get_id()
 	{
-		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT;
+		return MAVLINK_MSG_ID_ATTITUDE_TARGET;
 	}
 
 	static MavlinkStream *new_instance(Mavlink *mavlink)
 	{
-		return new MavlinkStreamRollPitchYawThrustSetpoint(mavlink);
+		return new MavlinkStreamAttitudeTarget(mavlink);
 	}
 
 	unsigned get_size()
 	{
-		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
+		return MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
 	}
 
 private:
-	MavlinkOrbSubscription *att_sp_sub;
-	uint64_t att_sp_time;
+	MavlinkOrbSubscription *_att_sp_sub;
+	MavlinkOrbSubscription *_att_rates_sp_sub;
+	uint64_t _att_sp_time;
+	uint64_t _att_rates_sp_time;
 
 	/* do not allow top copying this class */
-	MavlinkStreamRollPitchYawThrustSetpoint(MavlinkStreamRollPitchYawThrustSetpoint &);
-	MavlinkStreamRollPitchYawThrustSetpoint& operator = (const MavlinkStreamRollPitchYawThrustSetpoint &);
+	MavlinkStreamAttitudeTarget(MavlinkStreamAttitudeTarget &);
+	MavlinkStreamAttitudeTarget& operator = (const MavlinkStreamAttitudeTarget &);
 
 protected:
-	explicit MavlinkStreamRollPitchYawThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
-		att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
-		att_sp_time(0)
+	explicit MavlinkStreamAttitudeTarget(Mavlink *mavlink) : MavlinkStream(mavlink),
+		_att_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_attitude_setpoint))),
+		_att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
+		_att_sp_time(0),
+		_att_rates_sp_time(0)
 	{}
 
 	void send(const hrt_abstime t)
 	{
 		struct vehicle_attitude_setpoint_s att_sp;
 
-		if (att_sp_sub->update(&att_sp_time, &att_sp)) {
-			mavlink_roll_pitch_yaw_thrust_setpoint_t msg;
+		if (_att_sp_sub->update(&_att_sp_time, &att_sp)) {
+
+			struct vehicle_rates_setpoint_s att_rates_sp;
+			(void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp);
+
+			mavlink_attitude_target_t msg;
 
 			msg.time_boot_ms = att_sp.timestamp / 1000;
-			msg.roll = att_sp.roll_body;
-			msg.pitch = att_sp.pitch_body;
-			msg.yaw = att_sp.yaw_body;
+			mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q);
+
+			msg.body_roll_rate = att_rates_sp.roll;
+			msg.body_pitch_rate = att_rates_sp.pitch;
+			msg.body_yaw_rate = att_rates_sp.yaw;
+
 			msg.thrust = att_sp.thrust;
 
-			_mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_THRUST_SETPOINT, &msg);
-		}
-	}
-};
-
-
-class MavlinkStreamRollPitchYawRatesThrustSetpoint : public MavlinkStream
-{
-public:
-	const char *get_name() const
-	{
-		return MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static();
-	}
-
-	static const char *get_name_static()
-	{
-		return "ROLL_PITCH_YAW_RATES_THRUST_SETPOINT";
-	}
-
-	uint8_t get_id()
-	{
-		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT;
-	}
-
-	static MavlinkStream *new_instance(Mavlink *mavlink)
-	{
-		return new MavlinkStreamRollPitchYawRatesThrustSetpoint(mavlink);
-	}
-
-	unsigned get_size()
-	{
-		return MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES;
-	}
-
-private:
-	MavlinkOrbSubscription *_att_rates_sp_sub;
-	uint64_t _att_rates_sp_time;
-
-	/* do not allow top copying this class */
-	MavlinkStreamRollPitchYawRatesThrustSetpoint(MavlinkStreamRollPitchYawRatesThrustSetpoint &);
-	MavlinkStreamRollPitchYawRatesThrustSetpoint& operator = (const MavlinkStreamRollPitchYawRatesThrustSetpoint &);
-
-protected:
-	explicit MavlinkStreamRollPitchYawRatesThrustSetpoint(Mavlink *mavlink) : MavlinkStream(mavlink),
-		_att_rates_sp_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_rates_setpoint))),
-		_att_rates_sp_time(0)
-	{}
-
-	void send(const hrt_abstime t)
-	{
-		struct vehicle_rates_setpoint_s att_rates_sp;
-
-		if (_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp)) {
-			mavlink_roll_pitch_yaw_rates_thrust_setpoint_t msg;
-
-			msg.time_boot_ms = att_rates_sp.timestamp / 1000;
-			msg.roll_rate = att_rates_sp.roll;
-			msg.pitch_rate = att_rates_sp.pitch;
-			msg.yaw_rate = att_rates_sp.yaw;
-			msg.thrust = att_rates_sp.thrust;
-
-			_mavlink->send_message(MAVLINK_MSG_ID_ROLL_PITCH_YAW_RATES_THRUST_SETPOINT, &msg);
+			_mavlink->send_message(MAVLINK_MSG_ID_ATTITUDE_TARGET, &msg);
 		}
 	}
 };
@@ -2149,10 +2097,9 @@ StreamListItem *streams_list[] = {
 	new StreamListItem(&MavlinkStreamServoOutputRaw<2>::new_instance, &MavlinkStreamServoOutputRaw<2>::get_name_static),
 	new StreamListItem(&MavlinkStreamServoOutputRaw<3>::new_instance, &MavlinkStreamServoOutputRaw<3>::get_name_static),
 	new StreamListItem(&MavlinkStreamHILControls::new_instance, &MavlinkStreamHILControls::get_name_static),
-	new StreamListItem(&MavlinkStreamGlobalPositionSetpointInt::new_instance, &MavlinkStreamGlobalPositionSetpointInt::get_name_static),
+	new StreamListItem(&MavlinkStreamPositionTargetGlobalInt::new_instance, &MavlinkStreamPositionTargetGlobalInt::get_name_static),
 	new StreamListItem(&MavlinkStreamLocalPositionSetpoint::new_instance, &MavlinkStreamLocalPositionSetpoint::get_name_static),
-	new StreamListItem(&MavlinkStreamRollPitchYawThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawThrustSetpoint::get_name_static),
-	new StreamListItem(&MavlinkStreamRollPitchYawRatesThrustSetpoint::new_instance, &MavlinkStreamRollPitchYawRatesThrustSetpoint::get_name_static),
+	new StreamListItem(&MavlinkStreamAttitudeTarget::new_instance, &MavlinkStreamAttitudeTarget::get_name_static),
 	new StreamListItem(&MavlinkStreamRCChannelsRaw::new_instance, &MavlinkStreamRCChannelsRaw::get_name_static),
 	new StreamListItem(&MavlinkStreamManualControl::new_instance, &MavlinkStreamManualControl::get_name_static),
 	new StreamListItem(&MavlinkStreamOpticalFlow::new_instance, &MavlinkStreamOpticalFlow::get_name_static),
diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp
index 69e3ef31d7..63bac45c09 100644
--- a/src/modules/mavlink/mavlink_receiver.cpp
+++ b/src/modules/mavlink/mavlink_receiver.cpp
@@ -148,10 +148,6 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_vicon_position_estimate(msg);
 		break;
 
-	case MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST:
-		handle_message_quad_swarm_roll_pitch_yaw_thrust(msg);
-		break;
-
 	case MAVLINK_MSG_ID_RADIO_STATUS:
 		handle_message_radio_status(msg);
 		break;
@@ -362,36 +358,6 @@ MavlinkReceiver::handle_message_vicon_position_estimate(mavlink_message_t *msg)
 	}
 }
 
-void
-MavlinkReceiver::handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg)
-{
-	mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t swarm_offboard_control;
-	mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &swarm_offboard_control);
-
-	/* Only accept system IDs from 1 to 4 */
-	if (mavlink_system.sysid >= 1 && mavlink_system.sysid <= 4) {
-		struct offboard_control_setpoint_s offboard_control_sp;
-		memset(&offboard_control_sp, 0, sizeof(offboard_control_sp));
-
-		/* Convert values * 1000 back */
-		offboard_control_sp.p1 = (float)swarm_offboard_control.roll[mavlink_system.sysid - 1] / 1000.0f;
-		offboard_control_sp.p2 = (float)swarm_offboard_control.pitch[mavlink_system.sysid - 1] / 1000.0f;
-		offboard_control_sp.p3 = (float)swarm_offboard_control.yaw[mavlink_system.sysid - 1] / 1000.0f;
-		offboard_control_sp.p4 = (float)swarm_offboard_control.thrust[mavlink_system.sysid - 1] / 1000.0f;
-
-		offboard_control_sp.mode = (enum OFFBOARD_CONTROL_MODE)swarm_offboard_control.mode;
-
-		offboard_control_sp.timestamp = hrt_absolute_time();
-
-		if (_offboard_control_sp_pub < 0) {
-			_offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp);
-
-		} else {
-			orb_publish(ORB_ID(offboard_control_setpoint), _offboard_control_sp_pub, &offboard_control_sp);
-		}
-	}
-}
-
 void
 MavlinkReceiver::handle_message_radio_status(mavlink_message_t *msg)
 {

From 9ecec7fada7da3778c6214defcef68ea05352027 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Aug 2014 01:30:25 +0200
Subject: [PATCH 31/36] Add default initializers and timestamp in local
 position

---
 src/modules/mavlink/mavlink_messages.cpp                  | 7 ++++---
 src/modules/mc_pos_control/mc_pos_control_main.cpp        | 1 +
 src/modules/uORB/topics/vehicle_local_position_setpoint.h | 3 ++-
 3 files changed, 7 insertions(+), 4 deletions(-)

diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp
index 7f67475f2e..c10be77b52 100644
--- a/src/modules/mavlink/mavlink_messages.cpp
+++ b/src/modules/mavlink/mavlink_messages.cpp
@@ -1430,7 +1430,7 @@ protected:
 		struct position_setpoint_triplet_s pos_sp_triplet;
 
 		if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) {
-			mavlink_position_target_global_int_t msg;
+			mavlink_position_target_global_int_t msg{};
 
 			msg.coordinate_frame = MAV_FRAME_GLOBAL;
 			msg.lat_int = pos_sp_triplet.current.lat * 1e7;
@@ -1490,8 +1490,9 @@ protected:
 		struct vehicle_local_position_setpoint_s pos_sp;
 
 		if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) {
-			mavlink_position_target_local_ned_t msg;
+			mavlink_position_target_local_ned_t msg{};
 
+			msg.time_boot_ms = pos_sp.timestamp / 1000;
 			msg.coordinate_frame = MAV_FRAME_LOCAL_NED;
 			msg.x = pos_sp.x;
 			msg.y = pos_sp.y;
@@ -1558,7 +1559,7 @@ protected:
 			struct vehicle_rates_setpoint_s att_rates_sp;
 			(void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp);
 
-			mavlink_attitude_target_t msg;
+			mavlink_attitude_target_t msg{};
 
 			msg.time_boot_ms = att_sp.timestamp / 1000;
 			mavlink_euler_to_quaternion(att_sp.roll_body, att_sp.pitch_body, att_sp.yaw_body, msg.q);
diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp
index 0106af80aa..ecc40428de 100644
--- a/src/modules/mc_pos_control/mc_pos_control_main.cpp
+++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp
@@ -788,6 +788,7 @@ MulticopterPositionControl::task_main()
 			}
 
 			/* fill local position setpoint */
+			_local_pos_sp.timestamp = hrt_absolute_time();
 			_local_pos_sp.x = _pos_sp(0);
 			_local_pos_sp.y = _pos_sp(1);
 			_local_pos_sp.z = _pos_sp(2);
diff --git a/src/modules/uORB/topics/vehicle_local_position_setpoint.h b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
index 8988a0330d..6766bb58a9 100644
--- a/src/modules/uORB/topics/vehicle_local_position_setpoint.h
+++ b/src/modules/uORB/topics/vehicle_local_position_setpoint.h
@@ -50,11 +50,12 @@
  */
 
 struct vehicle_local_position_setpoint_s {
+	uint64_t timestamp;	/**< timestamp of the setpoint */
 	float x;		/**< in meters NED			  		*/
 	float y;		/**< in meters NED			  		*/
 	float z;		/**< in meters NED			  		*/
 	float yaw;		/**< in radians NED -PI..+PI  		*/
-}; /**< Local position in NED frame to go to */
+}; /**< Local position in NED frame */
 
 /**
  * @}

From 1d3edfa62976bdf73eeef67309c67b154be07810 Mon Sep 17 00:00:00 2001
From: Don Gagne <don@thegagnes.com>
Date: Sun, 10 Aug 2014 12:02:10 -0700
Subject: [PATCH 32/36] More FTP support

Added:
- sequence numbers
- directory list returns file sizes
- open command returns file size in data
---
 src/modules/mavlink/mavlink_ftp.cpp | 48 ++++++++++++++++++++++++-----
 src/modules/mavlink/mavlink_ftp.h   |  2 +-
 2 files changed, 42 insertions(+), 8 deletions(-)

diff --git a/src/modules/mavlink/mavlink_ftp.cpp b/src/modules/mavlink/mavlink_ftp.cpp
index 6a2c900afe..17d1babffe 100644
--- a/src/modules/mavlink/mavlink_ftp.cpp
+++ b/src/modules/mavlink/mavlink_ftp.cpp
@@ -35,6 +35,7 @@
 #include <unistd.h>
 #include <stdio.h>
 #include <fcntl.h>
+#include <sys/stat.h>
 
 #include "mavlink_ftp.h"
 
@@ -190,6 +191,8 @@ void
 MavlinkFTP::_reply(Request *req)
 {
 	auto hdr = req->header();
+	
+	hdr->magic = kProtocolMagic;
 
 	// message is assumed to be already constructed in the request buffer, so generate the CRC
 	hdr->crc32 = 0;
@@ -242,15 +245,18 @@ MavlinkFTP::_workList(Request *req)
 			break;
 		}
 
-		// name too big to fit?
-		if ((strlen(entry.d_name) + offset + 2) > kMaxDataLength) {
-			break;
-		}
+		uint32_t fileSize = 0;
+		char buf[256];
 
 		// store the type marker
 		switch (entry.d_type) {
 		case DTYPE_FILE:
 			hdr->data[offset++] = kDirentFile;
+			snprintf(buf, sizeof(buf), "%s/%s", dirPath, entry.d_name);
+			struct stat st;
+			if (stat(buf, &st) == 0) {
+				fileSize = st.st_size;
+			}
 			break;
 		case DTYPE_DIRECTORY:
 			hdr->data[offset++] = kDirentDir;
@@ -259,11 +265,24 @@ MavlinkFTP::_workList(Request *req)
 			hdr->data[offset++] = kDirentUnknown;
 			break;
 		}
+		
+		if (entry.d_type == DTYPE_FILE) {
+			snprintf(buf, sizeof(buf), "%s\t%d", entry.d_name, fileSize);
+		} else {
+			strncpy(buf, entry.d_name, sizeof(buf));
+			buf[sizeof(buf)-1] = 0;
+		}
+		size_t nameLen = strlen(buf);
 
+		// name too big to fit?
+		if ((nameLen + offset + 2) > kMaxDataLength) {
+			break;
+		}
+		
 		// copy the name, which we know will fit
-		strcpy((char *)&hdr->data[offset], entry.d_name);
+		strcpy((char *)&hdr->data[offset], buf);
 		//printf("FTP: list %s %s\n", dirPath, (char *)&hdr->data[offset-1]);
-		offset += strlen(entry.d_name) + 1;
+		offset += nameLen + 1;
 	}
 
 	closedir(dp);
@@ -282,6 +301,16 @@ MavlinkFTP::_workOpen(Request *req, bool create)
 		return kErrNoSession;
 	}
 
+	
+	uint32_t fileSize = 0;
+	if (!create) {
+		struct stat st;
+		if (stat(req->dataAsCString(), &st) != 0) {
+			return kErrNotFile;
+		}
+		fileSize = st.st_size;
+	}
+
 	int oflag = create ? (O_CREAT | O_EXCL | O_APPEND) : O_RDONLY;
     
 	int fd = ::open(req->dataAsCString(), oflag);
@@ -291,7 +320,12 @@ MavlinkFTP::_workOpen(Request *req, bool create)
 	_session_fds[session_index] = fd;
 
 	hdr->session = session_index;
-	hdr->size = 0;
+	if (create) {
+		hdr->size = 0;
+	} else {
+		hdr->size = sizeof(uint32_t);
+		*((uint32_t*)hdr->data) = fileSize;
+	}
 
 	return kErrNone;
 }
diff --git a/src/modules/mavlink/mavlink_ftp.h b/src/modules/mavlink/mavlink_ftp.h
index 1bd1158fba..800c98b69f 100644
--- a/src/modules/mavlink/mavlink_ftp.h
+++ b/src/modules/mavlink/mavlink_ftp.h
@@ -146,7 +146,7 @@ private:
 			mavlink_message_t msg;
 			msg.checksum = 0;
 			unsigned len = mavlink_msg_encapsulated_data_pack_chan(_mavlink->get_system_id(), _mavlink->get_component_id(),
-				_mavlink->get_channel(), &msg, sequence(), rawData());
+				_mavlink->get_channel(), &msg, sequence()+1, rawData());
 
 			_mavlink->lockMessageBufferMutex();
 			bool success = _mavlink->message_buffer_write(&msg, len);

From 0633d7f601b38a60c2b0ed1ca98d57bd3f80e482 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Sun, 10 Aug 2014 22:05:21 +0200
Subject: [PATCH 33/36] Updated mavlink version

---
 mavlink/include/mavlink/v1.0 | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/mavlink/include/mavlink/v1.0 b/mavlink/include/mavlink/v1.0
index 8cc21472cc..4cd384001b 160000
--- a/mavlink/include/mavlink/v1.0
+++ b/mavlink/include/mavlink/v1.0
@@ -1 +1 @@
-Subproject commit 8cc21472cce09709d4c651e2855d891843a17e41
+Subproject commit 4cd384001b5373e1ecaa6c4cd66994855cec4742

From 258fb27e146ad175ca89a84ebc9c340a53db0884 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 11 Aug 2014 14:54:18 +0200
Subject: [PATCH 34/36] Ensure that a failing dataman start does not abort boot

---
 ROMFS/px4fmu_common/init.d/rcS | 6 ++++--
 1 file changed, 4 insertions(+), 2 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 24b2a299af..72cd4b0e1e 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -280,9 +280,11 @@ then
 	nshterm /dev/ttyACM0 &
 
 	#
-	# Start the datamanager
+	# Start the datamanager (and do not abort boot if it fails)
 	#
-	dataman start
+	if dataman start
+	then
+	fi
 
 	#
 	# Start the Commander (needs to be this early for in-air-restarts)

From 2ca89032409d0519e242006fa34ab26548fdd7e0 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Mon, 11 Aug 2014 14:56:13 +0200
Subject: [PATCH 35/36] If we failed loading params, reset them (if the file
 handle is valid the file is corrupted and there is no hope, if its just not
 present the reset command will not do anything)

---
 ROMFS/px4fmu_common/init.d/rcS | 7 +++++--
 1 file changed, 5 insertions(+), 2 deletions(-)

diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS
index 72cd4b0e1e..c9e6a27cac 100644
--- a/ROMFS/px4fmu_common/init.d/rcS
+++ b/ROMFS/px4fmu_common/init.d/rcS
@@ -83,9 +83,12 @@ then
 	param select $PARAM_FILE
 	if param load
 	then
-		echo "[init] Params loaded: $PARAM_FILE"
+		echo "[param] Loaded: $PARAM_FILE"
 	else
-		echo "[init] ERROR: Params loading failed: $PARAM_FILE"
+		echo "[param] FAILED loading $PARAM_FILE"
+		if param reset
+		then
+		fi
 	fi
 
 	#

From 3f4aef60c88b1e570dd30bc47a13d5340073e9a9 Mon Sep 17 00:00:00 2001
From: Lorenz Meier <lm@inf.ethz.ch>
Date: Tue, 12 Aug 2014 08:21:38 +0200
Subject: [PATCH 36/36] Increase timeout in an attempt to prevent timout python
 failure

---
 Tools/px_uploader.py | 2 +-
 1 file changed, 1 insertion(+), 1 deletion(-)

diff --git a/Tools/px_uploader.py b/Tools/px_uploader.py
index cd7884f6d2..d8f4884bc0 100755
--- a/Tools/px_uploader.py
+++ b/Tools/px_uploader.py
@@ -180,7 +180,7 @@ class uploader(object):
 
         def __init__(self, portname, baudrate):
                 # open the port, keep the default timeout short so we can poll quickly
-                self.port = serial.Serial(portname, baudrate, timeout=0.5)
+                self.port = serial.Serial(portname, baudrate, timeout=2.0)
                 self.otp = b''
                 self.sn = b''